From c68caec626f712564659205db97900d5daa2de34 Mon Sep 17 00:00:00 2001 From: Greek Date: Mon, 9 Nov 2020 14:58:59 +0100 Subject: [PATCH] * Package update - New functions - Renames - New Definitions * rtps_handler overhaul - Validity Check for Submessages - OVERREAD Guard - Info Timestamp parsed and sent to Endpoints --- src/REF.txt | 159 ++++++ src/TODO.txt | 22 +- src/math_pkg.vhd | 1 + src/rtps_builtin_endpoint.vhd | 76 ++- src/rtps_config_package.vhd | 55 +- src/rtps_handler.vhd | 920 ++++++++++++++++++++++------------ src/rtps_package.vhd | 22 +- syn/DE10-Nano/top.qsf | 2 +- 8 files changed, 854 insertions(+), 403 deletions(-) diff --git a/src/REF.txt b/src/REF.txt index 3ee5f0a..a2be715 100644 --- a/src/REF.txt +++ b/src/REF.txt @@ -1,3 +1,12 @@ +RULES 8.3.4.1 (Message Receiver) +================================ +* If the full Submessage header cannot be read, the rest of the Message is considered invalid. +* If submessageLength field is invalid, the rest of the Message is invalid. +* A Submessage with an unknown SubmessageId must be ignored and parsing must continue with the next Submessage. +* The receiver of a Submessage should ignore unknown flags. +* A valid submessageLength field must always be used to find the next Submessage. +* A known but invalid Submessage invalidates the rest of the Message. + RULES 8.4.2 =========== @@ -85,10 +94,64 @@ ENDPOINT FIFO PACKET FORMAT | | +-------------------------------------------------------------+ | | ++ Timestamp + +| [only for DATA Submessage and User Destinations] | ++-------------------------------------------------------------+ +| | ~ PAYLOAD (SUBMESSAGE CONTENT) ~ | | +-------------------------------------------------------------+ +HEARTBEAT PAYLOAD +----------------- +31............24..............16..............8...............0 ++-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ ++-------------------------------------------------------------+ +| | ++ FirstSN + +| | ++-------------------------------------------------------------+ +| | ++ LastSN + +| | ++-------------------------------------------------------------+ +| Count | ++-------------------------------------------------------------+ + +ACKNACK PAYLOAD +--------------- +31............24..............16..............8...............0 ++-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ ++-------------------------------------------------------------+ +| | ++ ReaderSNState.BASE + +| | ++-------------------------------------------------------------+ +| ReaderSNState.NumBits | ++-------------------------------------------------------------+ +| [ReaderSNState.Bitmap] x 0-8 | ++-------------------------------------------------------------+ +| Count | ++-------------------------------------------------------------+ + +GAP PAYLOAD +----------- +31............24..............16..............8...............0 ++-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ ++-------------------------------------------------------------+ +| | ++ GapStart + +| | ++-------------------------------------------------------------+ +| | ++ GapList.BASE + +| | ++-------------------------------------------------------------+ +| GapList.NumBits | ++-------------------------------------------------------------+ +| [GapList.Bitmap] x 0-8 | ++-------------------------------------------------------------+ + ENDPOINT_ID =========== @@ -306,3 +369,99 @@ implementations must include an InfoTimestamp Submessage with every update from DDS_Advanced_Tutorial_2006_00-T1-2_Pardo.pdf (P.16) + +INVALIDATION +============ + +RTPS HEADER (8.3.6.3) +----------- +* The Message has less than the required number of octets to contain a full Header +* Its protocol value does not match the value of PROTOCOL_RTPS +* The major protocol version is larger than the major protocol version supported by the implementation + +ACKNACK (8.3.7.1.3) +------- +* submessageLength in the Submessage header is too small +* readerSNState is invalid + +DATA (8.3.7.2.3) +---- +* submessageLength in the Submessage header is too small +* writerSN.value is not strictly positive (1, 2, ...) or is SEQUENCENUMBER_UNKNOWN +* inlineQos is invalid + +DATA FRAG (8.3.7.3.3) +--------- +* submessageLength in the Submessage header is too small +* writerSN.value is not strictly positive (1, 2, ...) or is SEQUENCENUMBER_UNKNOWN +* fragmentStartingNum.value is not strictly positive (1, 2, ...) or exceeds the total number of fragments +* fragmentSize exceeds dataSize +* The size of serializedData exceeds (fragmentsInSubmessage * fragmentSize) +* inlineQos is invalid + +GAP (8.3.7.4.3) +--- +* submessageLength in the Submessage header is too small +* gapStart is zero or negative +* gapList is invalid +* (If GroupInfoFlag is set) gapStartGSN.value is zero or negative +* (If GroupInfoFlag is set) gapEndGSN.value is zero or negative +* (If GroupInfoFlag is set) gapEndGSN.value < gapStartGSN.value-1 + +HEARTBEAT (8.3.7.5.3) +--------- +* submessageLength in the Submessage header is too small +* firstSN.value is zero or negative +* lastSN.value is negative +* lastSN.value < firstSN.value - 1 +* (If GroupInfoFlag is set) currentGSN.value is zero or negative +* (If GroupInfoFlag is set) firstGSN.value is zero or negative +* (If GroupInfoFlag is set) lastGSN.value is negative +* (If GroupInfoFlag is set) lastGSN.value < firstGSN.value - 1 +* (If GroupInfoFlag is set) currentGSN.value < firstGSN.value +* (If GroupInfoFlag is set) currentGSN.value < lastGSN.value + +HEARTBEAT FRAG (8.3.7.6.3) +-------------- +* submessageLength in the Submessage header is too small +* writerSN.value is zero or negative +* lastFragmentNum.value is zero or negative + +INFO DESTINATION (8.3.7.7.3) +---------------- +* submessageLength in the Submessage header is too small + +INFO REPLY (8.3.7.8.3) +---------- +* submessageLength in the Submessage header is too small + +INFO SOURCE (8.3.7.9.3) +----------- +* submessageLength in the Submessage header is too small + +INFO TIMESTAMP (8.3.7.10.3) +-------------- +* submessageLength in the Submessage header is too small + +NACK FRAG (8.3.7.11.3) +--------- +* submessageLength in the Submessage header is too small +* writerSN.value is zero or negative +* fragmentNumberState is invalid + +PAD (8.3.7.12.3) +--- +ALWAYS VALID + +NumberSet (9.4.2.6) +--------- +* bitmapBase <= 0 +* numBits < 0 or numBits > 256 +* M/=(numBits+31)/32 longs in Submessage + +Parameter List (8.3.5.9) +-------------- +* There shall be no more than 2^16 possible values of the ParameterId_t parameterId +* A range of 2^15 values is reserved for protocol-defined parameters +* A range of 2^15 values is reserved for vendor-defined parameters +* The maximum length of any parameter is limited to 2^16 octets \ No newline at end of file diff --git a/src/TODO.txt b/src/TODO.txt index 998332d..8dd1971 100644 --- a/src/TODO.txt +++ b/src/TODO.txt @@ -48,12 +48,15 @@ But then, in 8.4.15.7 it says: "So, an implementation should ensure that same logical HEARTBEATs are tagged with the same Count." Does that mean there are cases were I have to put the same count? What is a logical HEARTBEAT? +* Should a "Keyed" Endpoint communicate with a "Non-Keyed"? +* Is the empty String a valid Topic and Type Name? +* We can determine if a Endpoint is a Reader or Writer via the Entity ID. Is it illegal to get a SEDP with incompatible source (Reader Entity ID from Publications Announcer?) * Fast-RTPS doen not follow DDSI-RTPS Specification - - Open Github Issue - https://github.com/eProsima/Fast-RTPS/issues/1221 - + - Open Github Issue + https://github.com/eProsima/Fast-RTPS/issues/1221 + - Seems that Fast-RTPS is also not checking the Validity of Submessages according to Spec * DDSI-RTPS 2.3 ISSUES - 8.3.7.7 InfoDestination @@ -64,10 +67,13 @@ writerSN is incorrectly shown as only 32 bits in width - 8.3.4 The RTPS Message Receiver, Table 8.16 - Initial State of the Receiver Port of UnicastReplyLocatorList should be initialized to Source Port. + - 8.3.7.4.3 Validity + gapList.Base >= gapStart - 8.3.7.10.3 Validity 'This Submessage is invalid when the following is true: submessageLength in the Submessage header is too small' But if InvalidateFlag is set, Length can be Zero. Since the length is unsigned, there cannot be an invalid length. + - 9.4.5.1.2 Flags Clarify from where the endianness begins. One might think it would begin after the Submessage Header, but the length is also endian dependent. @@ -101,5 +107,15 @@ DESIGN DECISIONS Use the lowest bit of the Heartbeat/Acknack Deadline stored in the Participant Data to differentiate between Delay and Suppression. This reduces the resolution from 0.23 ns to 0.47 ns +PROTOCOL UNCOMPLIANCE +===================== +* Partition QoS +* Built-in Endpoint is NOT the same as a normal Endpoint + -> No User access to Data +* Known but unused Submessage IDs are treated as uknown + -> No validity check +* Inline QoS validated in Endpoint + -> Cannot invalidate Rest of Message/Packet + -- Input FIFO Guard -- Output FIFO Guard \ No newline at end of file diff --git a/src/math_pkg.vhd b/src/math_pkg.vhd index fed84f2..3e58354 100644 --- a/src/math_pkg.vhd +++ b/src/math_pkg.vhd @@ -58,6 +58,7 @@ package body math_pkg is return ret_value; end function; + -- TODO: Rename to ceil_div, since we do not actually round function round_div(constant divident, divisor : in integer) return integer is variable ret : integer; begin diff --git a/src/rtps_builtin_endpoint.vhd b/src/rtps_builtin_endpoint.vhd index 4e251ec..1765c6d 100644 --- a/src/rtps_builtin_endpoint.vhd +++ b/src/rtps_builtin_endpoint.vhd @@ -115,10 +115,10 @@ architecture arch of rtps_builtin_endpoint is lease_deadline : TIME_TYPE; heartbeat_res_time : TIME_TYPE; acknack_res_time : TIME_TYPE; - spdp_seq_nr : SEQUENCE_NR_TYPE; - pub_seq_nr : SEQUENCE_NR_TYPE; - sub_seq_nr : SEQUENCE_NR_TYPE; - mes_seq_nr : SEQUENCE_NR_TYPE; + spdp_seq_nr : SEQUENCENUMBER_TYPE; + pub_seq_nr : SEQUENCENUMBER_TYPE; + sub_seq_nr : SEQUENCENUMBER_TYPE; + mes_seq_nr : SEQUENCENUMBER_TYPE; end record; --*****CONSTANT DECLARATION***** @@ -155,11 +155,9 @@ architecture arch of rtps_builtin_endpoint is -- Signifies if the Reader Endpoint expects in-line QoS constant EXPECTS_INLINE_QOS_FLAG : natural := 0; -- Highest Sequence Number of Publisher Data - constant PUB_SEQUENCE_NR : SEQUENCE_NR_TYPE := convert_to_double_word(to_unsigned(NUM_WRITERS, 64)); + constant PUB_SEQUENCENUMBER : SEQUENCENUMBER_TYPE := convert_to_double_word(to_unsigned(NUM_WRITERS, 64)); -- Highest Sequence Number of Subscriber Data - constant SUB_SEQUENCE_NR : SEQUENCE_NR_TYPE := convert_to_double_word(to_unsigned(NUM_READERS, 64)); - -- Constant for Sequence Number 1 - constant SEQUENCE_NR_START : SEQUENCE_NR_TYPE := convert_to_double_word(to_unsigned(1, 64)); + constant SUB_SEQUENCENUMBER : SEQUENCENUMBER_TYPE := convert_to_double_word(to_unsigned(NUM_READERS, 64)); -- Heartbeat/Liveliness Assertion Period constant HEARTBEAT_PERIOD : DURATION_TYPE := work.rtps_package.min(MIN_ENDPOINT_LEASE_DURATION, PARTICIPANT_HEARTBEAT_PERIOD) - DURATION_DELTA; -- Constant for zero Participant Data @@ -208,7 +206,7 @@ architecture arch of rtps_builtin_endpoint is -- Source GUID Latch signal guid, guid_next : GUID_TYPE := (others => (others => '0')); -- RTPS DATA Submessage Sequence Number Latch - signal seq_nr, seq_nr_next : SEQUENCE_NR_TYPE := (others => (others => '0')); + signal seq_nr, seq_nr_next : SEQUENCENUMBER_TYPE := (others => (others => '0')); -- Word aligned End of Parameter signal parameter_end, parameter_end_next : unsigned(PARAMETER_LENGTH_WIDTH-1 downto 0) := (others => '0'); -- RTPS DATA Submessage Content Type @@ -269,13 +267,13 @@ architecture arch of rtps_builtin_endpoint is -- General Purpose Counter (Memory FSM) signal mem_cnt, mem_cnt_next : natural range 0 to max(22, ENDPOINT_BITMASK_SIZE-1) := 0; -- Contains the Sequence Number stored in the Buffer of the current relevant Message Type (Participant/Publisher/Subscriber/Message Data) - signal mem_seq_nr, mem_seq_nr_next : SEQUENCE_NR_TYPE := (others => (others => '0')); + signal mem_seq_nr, mem_seq_nr_next : SEQUENCENUMBER_TYPE := (others => (others => '0')); -- Signifies the next expected Sequence Number of the current relevant Message Type (Participant/Publisher/Subscriber/Message Data) - signal next_seq_nr, next_seq_nr_next : SEQUENCE_NR_TYPE := (others => (others => '0')); + signal next_seq_nr, next_seq_nr_next : SEQUENCENUMBER_TYPE := (others => (others => '0')); -- Latch used to store the first Sequence Number in ACKNACK/HEARTBEAT/GAP Messages - signal first_seq_nr, first_seq_nr_next : SEQUENCE_NR_TYPE := (others => (others => '0')); + signal first_seq_nr, first_seq_nr_next : SEQUENCENUMBER_TYPE := (others => (others => '0')); -- Latch used to store the last Sequence Number in HEARTBEAT/GAP Messages - signal last_seq_nr, last_seq_nr_next : SEQUENCE_NR_TYPE := (others => (others => '0')); + signal last_seq_nr, last_seq_nr_next : SEQUENCENUMBER_TYPE := (others => (others => '0')); -- Signifies if we currently do a Orphan Endpoint Search (Endpoint whose parent Participant was removed) signal is_orphan_search, is_orphan_search_next : std_logic := '0'; -- Intermediate write enable signal. @@ -306,13 +304,13 @@ architecture arch of rtps_builtin_endpoint is signal reset_endpoint_alive : std_logic := '0'; -- NOTE: The "auto_live_seq_nr" is always higher than "man_live_seq_nr" -- Contains the highest Sequence Number for automatic liveliness updates - signal auto_live_seq_nr, auto_live_seq_nr_next : SEQUENCE_NR_TYPE := (others => (others => '0')); + signal auto_live_seq_nr, auto_live_seq_nr_next : SEQUENCENUMBER_TYPE := (others => (others => '0')); -- Contains the highest Sequence Number for manual by participant liveliness updates - signal man_live_seq_nr, man_live_seq_nr_next : SEQUENCE_NR_TYPE := (others => (others => '0')); + signal man_live_seq_nr, man_live_seq_nr_next : SEQUENCENUMBER_TYPE := (others => (others => '0')); -- Points to the first Sequence Number after "man_live_seq_nr" (Signifies the start of the GAP between "man_live_seq_nr" and "auto_live_seq_nr") - signal live_gap_start, live_gap_start_next : SEQUENCE_NR_TYPE := (others => (others => '0')); + signal live_gap_start, live_gap_start_next : SEQUENCENUMBER_TYPE := (others => (others => '0')); -- Points to the first Sequence Number before "auto_live_seq_nr" (Signifies the end of the GAP between "man_live_seq_nr" and "auto_live_seq_nr") - signal live_gap_end, live_gap_end_next : SEQUENCE_NR_TYPE := (others => (others => '0')); + signal live_gap_end, live_gap_end_next : SEQUENCENUMBER_TYPE := (others => (others => '0')); -- Participant Announcement Timeout Time signal announcement_time, announcement_time_next : TIME_TYPE := (others => (others => '0')); -- Heartbeat/Liveliness Assertion Timeout Time @@ -413,20 +411,8 @@ begin 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 (endian_flag = '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; + -- Big Endian Representation + data_in_swapped <= endian_swap(endian_flag, data_in); -- This process is responsible for toggling the "endpoint_alive" signal -- The signal is set high when at least one bit of the "alive" signal is high for at least one clock cycle, @@ -1301,7 +1287,7 @@ begin -- Subscriber Acknack if (is_subscriber = '1') then -- If Reader has not ACKed all Publisher History Cache - if (first_seq_nr <= PUB_SEQUENCE_NR) then + if (first_seq_nr <= PUB_SEQUENCENUMBER) then -- Set Acknack Response Time and Publisher Data as Acknack Response mem_opcode <= UPDATE_PARTICIPANT; deadline_next <= time + PARTICIPANT_ACKNACK_RESPONSE_DELAY; @@ -1315,7 +1301,7 @@ begin -- Publisher Acknack else -- If Reader has not ACKed all Subscriber History Cache - if (first_seq_nr <= SUB_SEQUENCE_NR) then + if (first_seq_nr <= SUB_SEQUENCENUMBER) then -- Set Acknack Response Time and set Subscriber Data as Acknack Response mem_opcode <= UPDATE_PARTICIPANT; deadline_next <= time + PARTICIPANT_ACKNACK_RESPONSE_DELAY; @@ -1354,7 +1340,7 @@ begin -- Publisher Data not scheduled for response if (mem_participant_data.extra_flags(PUB_DATA_FLAG) = '0') then -- If Reader has not ACKed all Publisher History Cache - if (first_seq_nr <= PUB_SEQUENCE_NR) then + if (first_seq_nr <= PUB_SEQUENCENUMBER) then -- Set Publisher Data as Acknack Response mem_opcode <= UPDATE_PARTICIPANT; extra_flags_next <= mem_participant_data.extra_flags; @@ -1368,7 +1354,7 @@ begin -- Subscriber Data not scheduled for response if (mem_participant_data.extra_flags(SUB_DATA_FLAG) = '0') then -- If Reader has not ACKed all Subscriber History Cache - if (first_seq_nr <= SUB_SEQUENCE_NR) then + if (first_seq_nr <= SUB_SEQUENCENUMBER) then -- Set Subscriber Data as Acknack Response mem_opcode <= UPDATE_PARTICIPANT; extra_flags_next <= mem_participant_data.extra_flags; @@ -1550,7 +1536,7 @@ begin -- Reset Word Counter reset_read_cnt <= '1'; -- Latch Parameter End (In order to skip parameters) - parameter_end_next <= unsigned(big_endian_swap(endian_flag,parameter_length)); + parameter_end_next <= unsigned(endian_swap(endian_flag,parameter_length)); -- DEFAULT STAGE stage_next <= SKIP_PARAMETER; @@ -1742,7 +1728,7 @@ begin when PID_PROPERTY_LIST => -- Ignore null; - when PID_TYPE_MAX_SIZE_SERIALIZED => + when PID_DATA_MAX_SIZE_SERIALIZED => -- Ignore null; when PID_ENTITY_NAME => @@ -2692,16 +2678,16 @@ begin output_sig <= ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER; -- Sequence Number 1/2 when 3 => - output_sig <= std_logic_vector(SEQUENCE_NR_START(0)); + output_sig <= std_logic_vector(FIRST_SEQUENCENUMBER(0)); -- Sequence Number 2/2 when 4 => - output_sig <= std_logic_vector(SEQUENCE_NR_START(1)); + output_sig <= std_logic_vector(FIRST_SEQUENCENUMBER(1)); -- Sequence Number 1/2 when 5 => - output_sig <= std_logic_vector(PUB_SEQUENCE_NR(0)); + output_sig <= std_logic_vector(PUB_SEQUENCENUMBER(0)); -- Sequence Number 1/2 when 6 => - output_sig <= std_logic_vector(PUB_SEQUENCE_NR(1)); + output_sig <= std_logic_vector(PUB_SEQUENCENUMBER(1)); -- Count when 7 => output_sig <= std_logic_vector(count); @@ -2717,16 +2703,16 @@ begin output_sig <= ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER; -- Sequence Number 1/2 when 11 => - output_sig <= std_logic_vector(SEQUENCE_NR_START(0)); + output_sig <= std_logic_vector(FIRST_SEQUENCENUMBER(0)); -- Sequence Number 2/2 when 12 => - output_sig <= std_logic_vector(SEQUENCE_NR_START(1)); + output_sig <= std_logic_vector(FIRST_SEQUENCENUMBER(1)); -- Sequence Number 1/2 when 13 => - output_sig <= std_logic_vector(SUB_SEQUENCE_NR(0)); + output_sig <= std_logic_vector(SUB_SEQUENCENUMBER(0)); -- Sequence Number 1/2 when 14 => - output_sig <= std_logic_vector(SUB_SEQUENCE_NR(1)); + output_sig <= std_logic_vector(SUB_SEQUENCENUMBER(1)); -- Count when 15 => output_sig <= std_logic_vector(count); diff --git a/src/rtps_config_package.vhd b/src/rtps_config_package.vhd index 6772fe7..7bb9ea9 100644 --- a/src/rtps_config_package.vhd +++ b/src/rtps_config_package.vhd @@ -16,6 +16,9 @@ package rtps_config_package is constant ENTITYID : ENTITYID_TYPE; -- Deferred to Package Body constant DOMAIN_ID : std_logic_vector(DOMAIN_ID_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(USER_DOMAIN_ID, DOMAIN_ID_WIDTH)); + -- Constant for Sequence Number 1 + constant FIRST_SEQUENCENUMBER : SEQUENCENUMBER_TYPE := convert_to_double_word(to_unsigned(1, 64)); + -- Smallest Writer Endpoint Lease Duration constant MIN_ENDPOINT_LEASE_DURATION : DURATION_TYPE; -- Deferred to package Body @@ -69,11 +72,19 @@ package rtps_config_package is constant ENDPOINT_TOPIC : ENDPOINT_STRING_TYPE; --Deferred to package body constant ENDPOINT_TYPE : ENDPOINT_STRING_TYPE; --Deferred to package body constant DOMAIN_TAG : STRING_WORD_ARRAY_TYPE; -- Deferred to package body + -- TODO: Use everywhere + constant EMPTY_STRING : STRING_WORD_ARRAY_TYPE := (others => (others => '0')); -- Swap "data" to Big Endian representation. - function big_endian_swap(is_little_endian : std_logic; data : std_logic_vector) return std_logic_vector; - function big_endian_swap(is_little_endian : std_logic; data : unsigned) return unsigned; + function endian_swap(swap : std_logic; data : std_logic_vector) return std_logic_vector; + function endian_swap(swap : std_logic; data : unsigned) return unsigned; + -- Return the Byte length of the string. (First NUL Byte is included) + function string_len (str : STRING_WORD_ARRAY_TYPE) return natural; + function boolean_to_std_logic(input : boolean) return std_logic; + function convert_string (str : string(1 to 256)) return STRING_WORD_ARRAY_TYPE; + + function round_slv(slv : std_logic_vector; width : natural) return std_logic_vector; end package; package body rtps_config_package is @@ -132,7 +143,7 @@ package body rtps_config_package is end loop; end procedure; - function booelan_to_std_logic(input : boolean) return std_logic is + function boolean_to_std_logic(input : boolean) return std_logic is variable ret : std_logic := '0'; begin ret := '0'; @@ -149,18 +160,18 @@ package body rtps_config_package is for i in 0 to ret'length-1 loop -- (see DDSI-RTPS 2.3 Section 9.3.1.2) -- Entity Kind Mapping - ret(i)(7 downto 6) := USER_DEFINED_ENTITY; + ret(i)(ENTITY_KIND_H_RANGE) := USER_DEFINED_ENTITY; if (i <= NUM_READERS-1) then if (ENDPOINT_WITH_KEY(i)) then - ret(i)(5 downto 0) := READER_WITH_KEY; + ret(i)(ENTITY_KIND_L_RANGE) := READER_WITH_KEY; else - ret(i)(5 downto 0) := READER_NO_KEY; + ret(i)(ENTITY_KIND_L_RANGE) := READER_NO_KEY; end if; else if (ENDPOINT_WITH_KEY(i)) then - ret(i)(5 downto 0) := WRITER_WITH_KEY; + ret(i)(ENTITY_KIND_L_RANGE) := WRITER_WITH_KEY; else - ret(i)(5 downto 0) := WRITER_NO_KEY; + ret(i)(ENTITY_KIND_L_RANGE) := WRITER_NO_KEY; end if; end if; -- ID Mapping @@ -385,7 +396,7 @@ package body rtps_config_package is len := len + 1; ret.data(ind+len) := ENDPOINT_PRESENTATION_QOS(i); len := len + 1; - ret.data(ind+len) := (24 => booelan_to_std_logic(ENDPOINT_COHERENT_ACCESS(i)), 16 => booelan_to_std_logic(ENDPOINT_ORDERED_ACCESS(i)), others => '0'); + ret.data(ind+len) := (24 => boolean_to_std_logic(ENDPOINT_COHERENT_ACCESS(i)), 16 => boolean_to_std_logic(ENDPOINT_ORDERED_ACCESS(i)), others => '0'); end if; -- SENTINEL len := len + 1; @@ -534,7 +545,7 @@ package body rtps_config_package is len := len + 1; ret.data(ind+len) := ENDPOINT_PRESENTATION_QOS(i); len := len + 1; - ret.data(ind+len) := (24 => booelan_to_std_logic(ENDPOINT_COHERENT_ACCESS(i)), 16 => booelan_to_std_logic(ENDPOINT_ORDERED_ACCESS(i)), others => '0'); + ret.data(ind+len) := (24 => boolean_to_std_logic(ENDPOINT_COHERENT_ACCESS(i)), 16 => boolean_to_std_logic(ENDPOINT_ORDERED_ACCESS(i)), others => '0'); end if; -- SENTINEL len := len + 1; @@ -677,7 +688,7 @@ package body rtps_config_package is 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)); + ret.data(ind+len) := PID_DEFAULT_MULTICAST_LOCATOR & std_logic_vector(to_unsigned(24, 16)); len := len + 1; ret.data(ind+len) := LOCATOR_KIND_UDPv4; len := len + 1; @@ -730,14 +741,14 @@ package body rtps_config_package is constant PARTICIPANT_DATA : OUTPUT_DATA_TYPE := gen_participant_data; -- Returns the 'data' argument either as is, or with reversed Byte order, depending on the - -- 'is_little_endian' argument. - function big_endian_swap(is_little_endian : std_logic; data : std_logic_vector) return std_logic_vector is + -- 'swap' argument. + function endian_swap(swap : 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 (is_little_endian = '1') then + if (swap = '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); @@ -750,10 +761,20 @@ package body rtps_config_package is end function; -- Returns the 'data' argument either as is, or with reversed Byte order, depending on the - -- 'is_little_endian' argument. - function big_endian_swap(is_little_endian : std_logic; data : unsigned) return unsigned is + -- 'swap' argument. + function endian_swap(swap : std_logic; data : unsigned) return unsigned is begin - return unsigned(big_endian_swap(is_little_endian, std_logic_vector(data))); + return unsigned(endian_swap(swap, std_logic_vector(data))); + end function; + + function round_slv(slv : std_logic_vector; width : natural) return std_logic_vector is + variable ret : std_logic_vector(width-1 downto 0) := (others => '0'); + begin + ret := slv(slv'length-1 downto slv'length-width); + if (slv(width-1 downto 0) /= (width-1 downto 0 => '0')) then + ret := std_logic_vector(unsigned(ret) + 1); + end if; + return ret; end function; end package body; diff --git a/src/rtps_handler.vhd b/src/rtps_handler.vhd index 87f55e9..7260158 100644 --- a/src/rtps_handler.vhd +++ b/src/rtps_handler.vhd @@ -8,7 +8,6 @@ use work.user_config.all; use work.rtps_config_package.all; -- Checksum has to be checked before --- TODO: Change "endian-swap" to "big-endian" throughout entity rtps_handler is port ( @@ -17,10 +16,9 @@ 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(WORD_WIDTH-1 downto 0); -- Input FIFO data signal - builtin_output : out std_logic_vector(WORD_WIDTH-1 downto 0); -- Output FIFO (Built-In Endpoint) data signal + data_out : out std_logic_vector(WORD_WIDTH-1 downto 0); -- Output 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(WORD_WIDTH-1 downto 0); -- Output FIFO (User Endpoints) data signal user_full : in std_logic_vector(0 to NUM_ENDPOINTS-1); -- Output FIFO (User Endpoints) full signal user_wr : out std_logic_vector(0 to NUM_ENDPOINTS-1); -- Output FIFO (User Endpoints) write signal last_word_out : out std_logic -- Output FIFO Last Word signal @@ -32,40 +30,13 @@ architecture arch of rtps_handler is --*****COMPONENT DECLARATION****** --*****CONSTANT DECLARATION***** - -- *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), 2 Count - constant MIN_ACKNACK_SIZE : natural := 22; - -- Minimum INFO_DST Submessage Size (Bytes) - -- 12 GUID Prefix - 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 : natural := 20; - -- Minimum INFO_REPLY Submessage Size (Bytes) - -- 4 LocatorList (4 numLocators) [Zero Size Locator List] - constant MIN_INFO_REPLY_SIZE : natural := 4; - -- Minimum INFO_REPLY_IP4 Submessage Size (Bytes) - -- 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 : natural := 28; - -- Minimum HEARTBEAT Submessage Size (Bytes) - -- 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 : 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, 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); + RTPS_HEADER_3, RTPS_SUB_HEADER, PARSE_INFO_DST, PARSE_INFO_SRC, PARSE_INFO_TS, PARSE_INFO_REPLY, EXTRACT_LOCATOR, + PARSE_INFO_REPLY_IP4, PARSE_HEARTBEAT, PARSE_ACKNACK, PARSE_GAP, PARSE_DATA, 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); @@ -74,7 +45,7 @@ architecture arch of rtps_handler is signal stage, 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 used to reset the 4-Byte Word counter signal reset_read_cnt : std_logic; -- 4-Byte Word counter (Counts words read from input FIFO) signal read_cnt : unsigned(UDP_HEADER_LENGTH_WIDTH-3 downto 0) := (others => '0'); @@ -100,38 +71,49 @@ architecture arch of rtps_handler is signal is_metatraffic, is_metatraffic_next : std_logic := '0'; -- RTPS Submessage Flag latch signal flags, flags_next : std_logic_vector(SUBMESSAGE_FLAGS_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 Entity ID latch + signal src_entityid, src_entityid_next : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0'); + -- Destination Entity ID latch + signal dest_entityid, dest_entityid_next : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0'); -- Source GUID Prefix latch signal src_guidprefix, src_guidprefix_next : GUIDPREFIX_TYPE := (others => (others => '0')); - -- Vector denoting the Destination User Endpoints of the Message + -- Vector denoting the Destination User Endpoints of the Submessage signal user_endpoint, user_endpoint_next : std_logic_vector(0 to NUM_ENDPOINTS-1) := (others => '0'); -- Denotes if the Message is destined for the Built-in Endpoints signal builtin_endpoint, builtin_endpoint_next : std_logic := '0'; - -- numLocator latch + -- LocatorList NumLocator latch -- NOTE: Since Submessages are limited to 2^16 Bytes, we can limit this also to 16 bits signal numlocators, numlocators_next : unsigned(SUBMESSAGE_LENGTH_WIDTH-1 downto 0) := (others => '0'); - -- Denotes if a suitable Locator has been found + -- Denotes if a suitable and valid Locator has been found signal locator_match, locator_match_next : std_logic := '0'; -- Denotes if the Source of the Message is a Reader Endpoint signal src_is_reader, src_is_reader_next : std_logic := '0'; - -- Intermediate Output Data Signal - signal output_sig : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0'); -- Intermediate Write Enable Signal signal wr_sig : std_logic := '0'; -- 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 : natural range 0 to 8 := 0; + signal cnt, cnt_next : natural range 0 to 10 := 0; -- Input in Big Endian representation signal data_in_swapped : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0'); -- Sequence Number latch - signal seq_nr, seq_nr_next : SEQUENCE_NR_TYPE := (others => (others => '0')); + signal sn_latch_1, sn_latch_1_next : SEQUENCENUMBER_TYPE := (others => (others => '0')); + -- Sequence Number latch + signal sn_latch_2, sn_latch_2_next : SEQUENCENUMBER_TYPE := (others => (others => '0')); + -- Sequence Number latch + signal sn_latch_3, sn_latch_3_next : SEQUENCENUMBER_TYPE := (others => (others => '0')); + -- Long atch + signal long_latch, long_latch_next : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0'); + -- Unsigned long latch + signal ulong_latch, ulong_latch_next : unsigned(CDR_LONG_WIDTH-1 downto 0) := (others => '0'); + -- NumberSet Bitmap latch + signal bitmap_latch, bitmap_latch_next : BITMAP_TYPE := (others => (others => '0')); + -- Counter used to read out Bitmaps + signal cnt2, cnt2_next : natural range 0 to BITMAP_TYPE'length := 0; + -- Denotes the number of valid Bitmap longs (4-Byte words) + signal bitmap_cnt, bitmap_cnt_next : unsigned(log2c(MAX_BITMAP_WIDTH/CDR_LONG_WIDTH)-1 downto 0) := (others => '0'); + -- Source Timestamp latch + signal src_ts, src_ts_next : TIME_TYPE := TIME_INVALID; -- Indicates the Destination IP Address signal ip_addr_type, ip_addr_type_next : IPv4_ADDRESS_TYPE := INVALID; -- Alias "substitution" @@ -150,64 +132,27 @@ architecture arch of rtps_handler is 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(big_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_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); + --alias rtps_sub_length : unsigned(15 downto 0) is unsigned(endian_swap(rtps_sub_flags(0), data_in(15 downto 0))); -- RTPS DATA SUBMESSAGE HEADER -- Apparently illegal alias expression - --alias rtps_sub_data_length : unsigned(15 downto 0) is unsigned(big_endian_swap(rtps_sub_endianness, data_in(15 downto 0))); + --alias rtps_sub_data_length : unsigned(15 downto 0) is unsigned(endian_swap(flags(SUBMESSAGE_ENDIAN_FLAG_POS), 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 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) + 1); - end if; - return tmp; - end function; - begin -- ALIAS SUBSTITUTION - rtps_sub_length <= unsigned(big_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(big_endian_swap(rtps_sub_endianness, data_in(15 downto 0))); + rtps_sub_length <= unsigned(endian_swap(rtps_sub_flags(SUBMESSAGE_ENDIAN_FLAG_POS), data_in(15 downto 0))); + rtps_sub_data_length <= unsigned(endian_swap(flags(SUBMESSAGE_ENDIAN_FLAG_POS), 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; + -- Big Endian Representation + data_in_swapped <= endian_swap(flags(SUBMESSAGE_ENDIAN_FLAG_POS), data_in); -- This process connects the Intermediate Output Signals to the actual output FIFOs output_prc : process(all) begin - -- Data Signal - builtin_output <= output_sig; - user_output <= output_sig; - --Write Enable Signal builtin_wr <= '0'; user_wr <= (others => '0'); @@ -222,7 +167,7 @@ begin -- 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". + -- 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. @@ -253,22 +198,26 @@ 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 - -- CHECK_GUID_PREFIX Validate Destination GUID Prefix - -- INFO_SRC_HEADER Parse INFO_SRC content - -- EXTRACT_LOCATOR_LIST Parse LocatorList Head - -- EXTRACT_LOCATOR Parse Locator - -- 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) + -- PARSE_INFO_DST Parse InfoDestination Submessage + -- PARSE_INFO_SRC Parse InfoSource Submessage + -- PARSE_INFO_TS Parse InfoTimestamp Submessage + -- PARSE_INFO_REPLY Parse InfoReply Submessage + -- EXTRACT_LOCATOR Parse Locator (of InfoReply Submessage) + -- PARSE_INFO_REPLY_IP4 Parse InfoReplyIP4 Submessage + -- PARSE_HEARTBEAT Parse Heartbeat Submessage + -- PARSE_ACKNACK Parse AckNack Submessage + -- PARSE_GAP Parse GAP Submessage + -- PARSE_DATA Parse DATA Submessage -- 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_PAYLOAD Read from input FIFO into relevant output FIFOs + -- PUSH_PAYLOAD Write Payload into relevant output FIFOs (May read directly from input FIFO) -- SKIP_SUB Skip rest of Submessage -- SKIP_PACKET Skip rest of UDP Packet parse_prc: process(all) - variable tmp : std_logic_vector(0 to NUM_ENDPOINTS-1) := (others => '0'); - variable dest : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0'); + variable tmp : std_logic_vector(0 to NUM_ENDPOINTS-1) := (others => '0'); + variable dest : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0'); + variable tmp_sn : SEQUENCENUMBER_TYPE := SEQUENCENUMBER_UNKNOWN; begin --DEFAULT Registered stage_next <= stage; @@ -281,9 +230,9 @@ begin src_addr_next <= src_addr; src_port_next <= src_port; flags_next <= flags; - reader_entityid_next <= reader_entityid; + src_entityid_next <= src_entityid; src_guidprefix_next <= src_guidprefix; - writer_entityid_next <= writer_entityid; + dest_entityid_next <= dest_entityid; user_endpoint_next <= user_endpoint; builtin_endpoint_next <= builtin_endpoint; numlocators_next <= numlocators; @@ -291,12 +240,19 @@ begin data_header_end_next <= data_header_end; locator_match_next <= locator_match; is_metatraffic_next <= is_metatraffic; - is_data_next <= is_data; src_is_reader_next <= src_is_reader; - seq_nr_next <= seq_nr; + sn_latch_1_next <= sn_latch_1; + sn_latch_2_next <= sn_latch_2; + sn_latch_3_next <= sn_latch_3; + long_latch_next <= long_latch; + ulong_latch_next <= ulong_latch; + bitmap_latch_next <= bitmap_latch; + cnt2_next <= cnt2; + bitmap_cnt_next <= bitmap_cnt; + src_ts_next <= src_ts; ip_addr_type_next <= ip_addr_type; -- DEFAULT Unregistered - output_sig <= (others => '0'); + data_out <= (others => '0'); rd_sig <= '0'; reset_read_cnt <= '0'; wr_sig <= '0'; @@ -309,6 +265,7 @@ begin -- Input FIFO Guard if (empty = '0') then rd_sig <= '1'; + reset_read_cnt <= '1'; src_addr_next <= data_in; @@ -318,7 +275,8 @@ begin when DEST_ADDR_HEADER => -- Input FIFO Guard if (empty = '0') then - rd_sig <= '1'; + rd_sig <= '1'; + reset_read_cnt <= '1'; case (data_in) is when DEFAULT_IPv4_META_ADDRESS => @@ -335,21 +293,16 @@ begin when LEN_HEADER => -- Input FIFO Guard if (empty = '0') then - rd_sig <= '1'; + rd_sig <= '1'; + -- NOTE: Read word counter starts counting the moment we leave this stage. (We can skip Packets from this stage on) + reset_read_cnt <= '1'; - -- Reset packet Byte Counter - reset_read_cnt <= '1'; - packet_length_next <= unsigned(data_in(13 downto 0)); + packet_length_next <= unsigned(data_in(packet_length'length-1 downto 0)); + -- Valid Submessage End + sub_end_next <= unsigned(data_in(sub_end'length-1 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 => @@ -361,11 +314,11 @@ begin is_metatraffic_next <= '0'; stage_next <= SKIP_PACKET; - src_port_next <= udp_src_port; + src_port_next <= udp_src_port; -- Check Address and Port Destination for validity case (ip_addr_type) is - -- Only accept Metatraffic form the Default Multicast Address + -- Only accept Metatraffic from the Default Multicast Address when DEFAULT_META_ADDR => is_metatraffic_next <= '1'; case (udp_dest_port) is @@ -407,7 +360,7 @@ begin -- 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 + if (round_slv(udp_length, packet_length'length) /= std_logic_vector(packet_length)) then -- Ignore stage_next <= SKIP_PACKET; else @@ -455,13 +408,15 @@ begin src_guidprefix_next(1) <= data_in; when 2 => src_guidprefix_next(2) <= data_in; + + -- Reset + src_ts_next <= TIME_INVALID; + stage_next <= RTPS_SUB_HEADER; when others => null; end case; end if; - -- NOTE: This Entity only parses the Destination Endpoint of the Submessages. The decoding - -- of the actual content is done in the respective Endpoints. -- RTPS Submessage Header (Fields: Submessage ID, Flags, Submessage Length) when RTPS_SUB_HEADER => -- Input FIFO Guard @@ -470,7 +425,6 @@ begin -- DEFAULT src_is_reader_next <= '0'; - is_data_next <= '0'; flags_next <= rtps_sub_flags; opcode_next <= rtps_sub_id; @@ -491,129 +445,66 @@ begin case (rtps_sub_id) is -- INFO_DST (Writer -> Reader, Update Destination GUID Prefix) - -- STAGE ORDER: CHECK_GUID_PREFIX -> SKIP_SUB -> RTPS_SUB_HEADER when SID_INFO_DST => - -- 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 - stage_next <= CHECK_GUID_PREFIX; - cnt_next <= 0; - end if; + stage_next <= PARSE_INFO_DST; + cnt_next <= 0; -- INFO_SRC (RTPS Header in Submessage form) - -- STAGE ORDER: INFO_SRC_HEADER -> SKIP_SUB -> RTPS_SUB_HEADER when SID_INFO_SRC => - -- 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 - stage_next <= INFO_SRC_HEADER; - cnt_next <= 0; - end if; + stage_next <= PARSE_INFO_SRC; + cnt_next <= 0; -- INFO_TS (Source Timestamp) when SID_INFO_TS => - -- IGNORE - stage_next <= SKIP_SUB; + -- Invalidate Stored Timestamp + if (rtps_sub_flags(SUBMESSAGE_INVALIDATE_FLAG_POS) = '1') then + src_ts_next <= TIME_INVALID; + stage_next <= SKIP_SUB; + else + stage_next <= PARSE_INFO_TS; + cnt_next <= 0; + end if; -- INFO_REPLY (Source Port and Address) - -- STAGE ORDER: EXTRACT_LOCATOR_LIST <-> EXTRACT_LOCATOR -> SKIP_SUB -> RTPS_SUB_HEADER when SID_INFO_REPLY => - -- 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 - stage_next <= EXTRACT_LOCATOR_LIST; - end if; + stage_next <= PARSE_INFO_REPLY; -- INFO_REPLY (Source Port and Address) - -- STAGE ORDER: EXTRACT_LOCATOR_UDPv4 -> SKIP_SUB -> RTPS_SUB_HEADER when SID_INFO_REPLY_IP4 => - -- 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 - stage_next <= EXTRACT_LOCATOR_UDPv4; - cnt_next <= 0; - end if; + stage_next <= PARSE_INFO_REPLY_IP4; + cnt_next <= 0; -- Heartbeat (Writer -> Reader, Available SeqNum) - -- STAGE ORDER: LATCH_SUBMESSAGE -> MATCH_DST_ENDPOINT -> PUSH_PAYLOAD_HEADER -> PUSH_PAYLOAD -> RTPS_SUB_HEADER when SID_HEARTBEAT => - -- 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 - stage_next <= LATCH_SUBMESSAGE; - cnt_next <= 0; - end if; + stage_next <= PARSE_HEARTBEAT; + cnt_next <= 0; -- AckNack (Reader -> Writer, Request SeqNum) - -- STAGE ORDER: LATCH_SUBMESSAGE -> MATCH_DST_ENDPOINT -> PUSH_PAYLOAD_HEADER -> PUSH_PAYLOAD -> RTPS_SUB_HEADER when SID_ACKNACK => - -- 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 - -- ACKNACK has Reader as Source - src_is_reader_next <= '1'; - - stage_next <= LATCH_SUBMESSAGE; - cnt_next <= 0; - end if; + -- Even though this Submessage is Ignored, it is parsed to check Validity + stage_next <= PARSE_ACKNACK; + cnt_next <= 0; -- GAP (Writer -> Reader, Invalidate SeqNum) - -- STAGE ORDER: LATCH_SUBMESSAGE -> MATCH_DST_ENDPOINT -> PUSH_PAYLOAD_HEADER -> PUSH_PAYLOAD -> RTPS_SUB_HEADER when SID_GAP => - -- 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 - stage_next <= LATCH_SUBMESSAGE; - cnt_next <= 0; - end if; + stage_next <= PARSE_GAP; + cnt_next <= 0; -- DATA (Writer -> Reader, SeqNum+Data) - -- STAGE ORDER: DATA_HEADER -> LATCH_SUBMESSAGE -> SKIP_DATA_HEADER -> MATCH_DST_ENDPOINT -> PUSH_PAYLOAD_HEADER -> PUSH_PAYLOAD -> RTPS_SUB_HEADER - when SID_DATA => - -- 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) + when SID_DATA => + stage_next <= PARSE_DATA; + cnt_next <= 0; + + -- SANITY CHECK + if (rtps_sub_flags(SUBMESSAGE_INLINE_QOS_FLAG_POS) = '0' and rtps_sub_flags(SUBMESSAGE_DATA_FLAG_POS) = '0' and rtps_sub_flags(SUBMESSAGE_KEY_FLAG_POS) = '0') then + -- Submessage has no Data, skip + stage_next <= SKIP_SUB; + -- VALIDITY CHECK + elsif (rtps_sub_flags(SUBMESSAGE_DATA_FLAG_POS) = '1' and rtps_sub_flags(SUBMESSAGE_KEY_FLAG_POS) = '1') then + -- Invalid Submessage, skip Packet (see DDSI-RTPS 2.3 Section 9.4.5.3.1 and 8.3.4.1) stage_next <= SKIP_PACKET; - else - is_data_next <= '1'; - - 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; - elsif (rtps_sub_data = '1' and rtps_sub_key = '1') then - -- Invalid Submessage, skip (see DDSI-RTPS 2.3 Section 9.4.5.3.1) - stage_next <= SKIP_SUB; - end if; end if; - when SID_HEARTBEAT_FRAG => - -- IGNORE - stage_next <= SKIP_SUB; - when SID_NACK_FRAG => - -- IGNORE - stage_next <= SKIP_SUB; - when SID_DATA_FRAG => - -- IGNORE - stage_next <= SKIP_SUB; -- PAD (Variable Size Padding) when SID_PAD => - -- IGNORE stage_next <= SKIP_SUB; -- Unknown ID, skip submessage when others => stage_next <= SKIP_SUB; end case; end if; - when CHECK_GUID_PREFIX => + when PARSE_INFO_DST => -- Input FIFO Guard if (empty = '0') then rd_sig <= '1'; @@ -645,7 +536,7 @@ begin null; end case; end if; - when INFO_SRC_HEADER => + when PARSE_INFO_SRC => -- Input FIFO Guard if (empty = '0') then rd_sig <= '1'; @@ -676,11 +567,29 @@ begin null; end case; end if; - when EXTRACT_LOCATOR_LIST => + when PARSE_INFO_TS => + rd_sig <= '1'; + cnt_next <= cnt + 1; + + case (cnt) is + -- Timesatmp 1/2 + when 0 => + src_ts_next(0) <= unsigned(data_in_swapped); + -- Timesatmp 2/2 + when 1 => + src_ts_next(1) <= unsigned(data_in_swapped); + + -- DONE + stage_next <= SKIP_SUB; + when others => + null; + end case; + when PARSE_INFO_REPLY => -- Input FIFO Guard if (empty = '0') then rd_sig <= '1'; + -- Extract Locator List numlocators_next <= unsigned(data_in_swapped(numlocators_next'length-1 downto 0)); stage_next <= EXTRACT_LOCATOR; @@ -690,11 +599,11 @@ begin -- Check Number of Locators if (numlocators = 0) then -- Check if Multicast Locator List exists - if (rtps_sub_multicast = '1') then + if (flags(SUBMESSAGE_MULTICAST_FLAG_POS) = '1') then -- Reset Flag to prevent loop - rtps_sub_multicast_next <= '0'; + flags_next(SUBMESSAGE_MULTICAST_FLAG_POS) <= '0'; - stage_next <= EXTRACT_LOCATOR_LIST; + stage_next <= PARSE_INFO_REPLY; -- No further Locators, next Submessage else stage_next <= SKIP_SUB; @@ -739,8 +648,6 @@ begin -- We only store valid UDPv4 Locators if (locator_match = '1' and data_in_swapped /= IPv4_ADDRESS_INVALID) then src_addr_next <= data_in_swapped; - -- Extract only first matching Locator and ignore the rest - stage_next <= SKIP_SUB; end if; -- Last Word of Locator numlocators_next <= numlocators - 1; @@ -750,7 +657,7 @@ begin end case; end if; end if; - when EXTRACT_LOCATOR_UDPv4 => + when PARSE_INFO_REPLY_IP4 => -- Input FIFO Guard if (empty = '0') then rd_sig <= '1'; @@ -771,65 +678,261 @@ begin -- Store only valid Locators if (locator_match = '1' and data_in_swapped(UDP_PORT_INVALID'range) /= UDP_PORT_INVALID) then src_port_next <= data_in_swapped(src_port_next'length-1 downto 0); - stage_next <= SKIP_SUB; + end if; + -- Parse Multicast if available - elsif (rtps_sub_multicast = '1') then + if (flags(SUBMESSAGE_MULTICAST_FLAG_POS) = '1') then locator_match_next <= '0'; cnt_next <= 0; - -- No valid Locators else + -- DONE stage_next <= SKIP_SUB; end if; when others => null; end case; end if; - -- DATA Submessage header (Extra Flags, octetstoinlineQoS) - when DATA_HEADER => - -- Input FIFO Guard - if (empty = '0') then - rd_sig <= '1'; - - -- 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 => + when PARSE_HEARTBEAT => -- 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 => + dest_entityid_next <= data_in; + -- Writer Entity ID + when 1 => + src_entityid_next <= data_in; + -- First Sequence Number 1/2 + when 2 => + sn_latch_1_next(0) <= unsigned(data_in_swapped); + -- First Sequence Number 2/2 + when 3 => + sn_latch_1_next(1) <= unsigned(data_in_swapped); + + -- VALIDITY CHECK + tmp_sn := (0 => sn_latch_1(0), 1 => unsigned(data_in_swapped)); + if (tmp_sn = 0 or tmp_sn(0)(WORD_WIDTH-1) = '1') then + -- If firstSN.value is zero or negative, skip Packet (see DDSI-RTPS 2.3 Section 8.3.7.5.3 and 8.3.4.1) + stage_next <= SKIP_PACKET; + end if; + -- Last Sequence Number 1/2 + when 4 => + sn_latch_2_next(0) <= unsigned(data_in_swapped); + + -- Pre-Calculation for Validity Check + sn_latch_3_next <= sn_latch_1 - 1; + -- Last Sequence Number 2/2 + when 5 => + sn_latch_2_next(1) <= unsigned(data_in_swapped); + + -- VALIDITY CHECK + tmp_sn := (0 => sn_latch_2(0), 1 => unsigned(data_in_swapped)); + if (tmp_sn(0)(WORD_WIDTH-1) = '1') then + -- If lastSN.value is negative, skip Packet (see DDSI-RTPS 2.3 Section 8.3.7.5.3 and 8.3.4.1) + stage_next <= SKIP_PACKET; + end if; + -- Count + when 6 => + long_latch_next <= data_in_swapped; + + if (sn_latch_2 < sn_latch_3) then + -- If lastSN.value < firstSN.value - 1, skip Packet (see DDSI-RTPS 2.3 Section 8.3.7.5.3 and 8.3.4.1) + stage_next <= SKIP_PACKET; + else + -- DONE + stage_next <= MATCH_DST_ENDPOINT; + end if; + when others => + null; + end case; + end if; + when PARSE_ACKNACK => + -- Input FIFO Guard + if (empty = '0') then + rd_sig <= '1'; + cnt_next <= cnt + 1; case (cnt) is -- Reader Entity ID when 0 => - reader_entityid_next <= data_in; + src_is_reader_next <= '1'; + src_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 + dest_entityid_next <= data_in; + -- ReaderSNState.Base 1/2 + when 2 => + sn_latch_1_next(0) <= unsigned(data_in_swapped); + -- ReaderSNState.Base 2/2 + when 3 => + sn_latch_1_next(1) <= unsigned(data_in_swapped); + + -- VALIDITY CHECK + tmp_sn := (0 => sn_latch_1(0), 1 => unsigned(data_in_swapped)); + if (tmp_sn = 0 or tmp_sn(0)(WORD_WIDTH-1) = '1') then + -- If Bitmap Base is zero or negative, Number Set is invalid (see DDSI-RTPS 2.3 Section 9.4.2.6) + -- If readerSNState is invalid, skip Packet (see DDSI-RTPS 2.3 Section 8.3.7.1.3 and 8.3.4.1) + stage_next <= SKIP_PACKET; + end if; + -- ReaderSNState.NumBits + when 4 => + ulong_latch_next <= unsigned(data_in_swapped); + -- XXX: Possible Worst Case Path (Comparison and two Arithmetic Operations in same Clock Cycle) + bitmap_cnt_next <= unsigned(round_slv(data_in_swapped(log2c(MAX_BITMAP_WIDTH)-1 downto 0),bitmap_cnt'length)) - 1; + + -- VALIDITY CHECK + if (data_in_swapped(0) = '1' or unsigned(data_in_swapped) > 256) then + -- If numBits is negative or larger than 256, Number Set is invalid (see DDSI-RTPS 2.3 Section 9.4.2.6) + -- If readerSNState is invalid, skip Packet (see DDSI-RTPS 2.3 Section 8.3.7.1.3 and 8.3.4.1) + stage_next <= SKIP_PACKET; + -- Bitmap empty + elsif (unsigned(data_in_swapped) = 0) then + -- Skip to Count + cnt_next <= 6; + else + -- Parse Bitmap + cnt2_next <= 0; + end if; + -- ReaderSNState.Bitmap + when 5 => + cnt2_next <= cnt2 + 1; + + bitmap_latch_next(cnt2) <= data_in_swapped; + + -- Keep Sub-State until whole Bitmap is read + if (cnt2 /= bitmap_cnt) then + cnt_next <= cnt; + end if; + -- Count + when 6 => + long_latch_next <= data_in_swapped; + + -- DONE + stage_next <= MATCH_DST_ENDPOINT; + when others => + null; + end case; + end if; + when PARSE_GAP => + -- Input FIFO Guard + if (empty = '0') then + rd_sig <= '1'; + cnt_next <= cnt + 1; + + case (cnt) is + -- Reader Entity ID + when 0 => + dest_entityid_next <= data_in; + -- Writer Entity ID + when 1 => + src_entityid_next <= data_in; + -- GapStart Sequence Number 1/2 + when 2 => + sn_latch_1_next(0) <= unsigned(data_in_swapped); + -- GapStart Sequence Number 2/2 + when 3 => + sn_latch_1_next(0) <= unsigned(data_in_swapped); + + -- VALIDITY CHECK + tmp_sn := (0 => sn_latch_1(0), 1 => unsigned(data_in_swapped)); + if (tmp_sn = 0 or tmp_sn(0)(WORD_WIDTH-1) = '1') then + -- If Bitmap Base is zero or negative, Number Set is invalid (see DDSI-RTPS 2.3 Section 9.4.2.6) + -- If gapList is invalid, skip Packet (see DDSI-RTPS 2.3 Section 8.3.7.4.3 and 8.3.4.1) + stage_next <= SKIP_PACKET; + end if; + -- GapList.Base 1/2 + when 4 => + sn_latch_2_next(0) <= unsigned(data_in_swapped); + -- GapList.Base 2/2 + when 5 => + sn_latch_2_next(1) <= unsigned(data_in_swapped); + + -- VALIDITY CHECK + tmp_sn := (0 => sn_latch_2(0), 1 => unsigned(data_in_swapped)); + if (tmp_sn = 0 or tmp_sn(0)(WORD_WIDTH-1) = '1') then + -- If Bitmap Base is zero or negative, Number Set is invalid (see DDSI-RTPS 2.3 Section 9.4.2.6) + -- If gapList is invalid, skip Packet (see DDSI-RTPS 2.3 Section 8.3.7.4.3 and 8.3.4.1) + stage_next <= SKIP_PACKET; + end if; + -- ReaderSNState.NumBits + when 6 => + ulong_latch_next <= unsigned(data_in_swapped); + -- XXX: Possible Worst Case Path (Comparison and two Arithmetic Operations in same Clock Cycle) + bitmap_cnt_next <= unsigned(round_slv(data_in_swapped(log2c(MAX_BITMAP_WIDTH)-1 downto 0),bitmap_cnt'length)) - 1; + + -- VALIDITY CHECK + if (data_in_swapped(0) = '1' or unsigned(data_in_swapped) > 256) then + -- If numBits is negative or larger than 256, Number Set is invalid (see DDSI-RTPS 2.3 Section 9.4.2.6) + -- If gapList is invalid, skip Packet (see DDSI-RTPS 2.3 Section 8.3.7.4.3 and 8.3.4.1) + stage_next <= SKIP_PACKET; + -- Bitmap empty + elsif(unsigned(data_in_swapped) = 0) then + -- DONE + stage_next <= MATCH_DST_ENDPOINT; + else + -- Parse Bitmap + cnt2_next <= 0; + end if; + -- ReaderSNState.Bitmap + when 7 => + cnt2_next <= cnt2 + 1; + + bitmap_latch_next(cnt2) <= data_in_swapped; + + -- Keep Sub-State until whole Bitmap is read + if (cnt2 /= bitmap_cnt) then + cnt_next <= cnt; + else + -- DONE stage_next <= MATCH_DST_ENDPOINT; end if; - -- Sequence Number 1/2 + when others => + null; + end case; + end if; + when PARSE_DATA => + -- Input FIFO Guard + if (empty = '0') then + rd_sig <= '1'; + cnt_next <= cnt + 1; + + case (cnt) is + -- Extra Flags & octetstoinlineQoS + when 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(rtps_sub_data_length(1 downto 0)); + data_header_end_next <= (read_cnt & "00") + rtps_sub_data_length; + -- Reader Entity ID + when 1 => + dest_entityid_next <= data_in; + -- Writer Entity ID when 2 => - seq_nr_next(0) <= unsigned(data_in); - -- Sequence Number 2/2 + src_entityid_next <= data_in; + -- Sequence Number 1/2 when 3 => - seq_nr_next(1) <= unsigned(data_in); - stage_next <= SKIP_DATA_HEADER; + sn_latch_1_next(0) <= unsigned(data_in_swapped); + -- Sequence Number 2/2 + when 4 => + sn_latch_1_next(1) <= unsigned(data_in_swapped); + + -- VALIDITY CHECK + tmp_sn := (0 => sn_latch_1(0), 1 => unsigned(data_in_swapped)); + if (tmp_sn = 0 or tmp_sn(0)(WORD_WIDTH-1) = '1') then + -- If Sequence Number is zero or negative, skip Packet (see DDSI-RTPS 2.3 Section 8.3.7.2.3 and 8.3.4.1) + -- NOTE: The actual wording used is "not strictly positive (1, 2, ...) or is SEQUENCENUMBER_UNKNOWN" + stage_next <= SKIP_PACKET; + else + stage_next <= SKIP_DATA_HEADER; + end if; 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 @@ -849,12 +952,6 @@ begin if (empty = '0') then rd_sig <= '1'; - 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'; @@ -863,7 +960,7 @@ begin -- *Check Dest Entity ID* -- Target ALL Endpoints - if (dest = ENTITYID_UNKNOWN) then + if (dest_entityid = ENTITYID_UNKNOWN) then if (is_metatraffic = '1') then builtin_endpoint_next <= '1'; else @@ -876,7 +973,7 @@ begin end if; end if; -- Target Built-In Endpoints - elsif (is_metatraffic = '1' and dest(7 downto 6) = BUILT_IN_ENTITY) then + elsif (is_metatraffic = '1' and dest_entityid(7 downto 6) = BUILT_IN_ENTITY) then builtin_endpoint_next <= '1'; -- Match User Entity ID elsif (is_metatraffic = '0') then @@ -884,7 +981,7 @@ begin tmp := (others => '0'); for i in 0 to ENTITYID'length-1 loop - if (dest = ENTITYID(i)) then + if (dest_entityid = ENTITYID(i)) then tmp(i) := '1'; end if; end loop; @@ -917,111 +1014,254 @@ begin case (cnt) is -- OPCODE (Submessage ID), Submessage Flags, UPDv4 Source Port when 0 => - output_sig <= opcode & flags & src_port; + data_out <= opcode & flags & src_port; wr_sig <= '1'; -- IPv4 Source Address when 1 => - output_sig <= src_addr; + data_out <= src_addr; wr_sig <= '1'; -- Source Entity ID when 2 => - if (src_is_reader = '1') then - output_sig <= reader_entityid; - else - output_sig <= writer_entityid; - end if; + data_out <= src_entityid; wr_sig <= '1'; -- GUID Prefix 1/3 when 3 => - output_sig <= src_guidprefix(0); + data_out <= src_guidprefix(0); wr_sig <= '1'; -- GUID Prefix 2/3 when 4 => - output_sig <= src_guidprefix(1); + data_out <= src_guidprefix(1); wr_sig <= '1'; -- GUID Prefix 3/3 when 5 => - output_sig <= src_guidprefix(2); + data_out <= 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; + data_out <= dest_entityid; 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)); + if (opcode = SID_DATA) then + data_out <= std_logic_vector(sn_latch_1(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)); + if (opcode = SID_DATA) then + data_out <= std_logic_vector(sn_latch_1(1)); + wr_sig <= '1'; + end if; + -- Source Timestamp 1/2 [only for DATA Submessages and User Endpoints] + when 9 => + if (opcode = SID_DATA and builtin_endpoint = '0') then + data_out <= std_logic_vector(src_ts(0)); + wr_sig <= '1'; + end if; + -- Source Timestamp 1/2 [only for DATA Submessages and User Endpoints] + when 10 => + if (opcode = SID_DATA and builtin_endpoint = '0') then + data_out <= std_logic_vector(src_ts(1)); wr_sig <= '1'; end if; - stage_next <= PUSH_PAYLOAD; + stage_next <= PUSH_PAYLOAD; + cnt_next <= 0; when others => null; end case; end if; when PUSH_PAYLOAD => - -- Input FIFO Guard - if (empty = '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'; + -- 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 (opcode) is + when SID_HEARTBEAT => + case (cnt) is + -- FirstSN 1/2 + when 0 => + data_out <= std_logic_vector(sn_latch_1(0)); + wr_sig <= '1'; + -- FirstSN 2/2 + when 1 => + data_out <= std_logic_vector(sn_latch_1(1)); + wr_sig <= '1'; + -- LastSN 1/2 + when 2 => + data_out <= std_logic_vector(sn_latch_2(0)); + wr_sig <= '1'; + -- LastSN 2/2 + when 3 => + data_out <= std_logic_vector(sn_latch_2(1)); + wr_sig <= '1'; + -- Count + when 4 => + data_out <= long_latch; + wr_sig <= '1'; + last_word_out <= '1'; + + -- DONE + stage_next <= SKIP_SUB; + when others => + null; + end case; + when SID_ACKNACK => + case (cnt) is + -- ReaderSNState.Base 1/2 + when 0 => + data_out <= std_logic_vector(sn_latch_1(0)); + wr_sig <= '1'; + -- ReaderSNState.Base 2/2 + when 1 => + data_out <= std_logic_vector(sn_latch_1(1)); + wr_sig <= '1'; + -- ReaderSNState.NumBits + when 2 => + data_out <= std_logic_vector(ulong_latch); + wr_sig <= '1'; + cnt2_next <= 0; + -- ReaderSNState.Bitmap + when 3 => + cnt2_next <= cnt2 + 1; + + data_out <= bitmap_latch_next(cnt2); + wr_sig <= '1'; + + -- Keep Sub-State until Bitmap is written + if (cnt2 /= bitmap_cnt) then + cnt_next <= cnt; + end if; + -- Count + when 4 => + data_out <= long_latch; + wr_sig <= '1'; + last_word_out <= '1'; + + -- DONE + stage_next <= SKIP_SUB; + when others => + null; + end case; + when SID_GAP => + case (cnt) is + -- GapStart 1/2 + when 0 => + data_out <= std_logic_vector(sn_latch_1(0)); + wr_sig <= '1'; + -- GapStart 2/2 + when 1 => + data_out <= std_logic_vector(sn_latch_1(1)); + wr_sig <= '1'; + -- GapList.Base 1/2 + when 2 => + data_out <= std_logic_vector(sn_latch_2(0)); + wr_sig <= '1'; + -- GapList.Base 2/2 + when 3 => + data_out <= std_logic_vector(sn_latch_2(1)); + wr_sig <= '1'; + -- GapList.NumBits + when 4 => + data_out <= std_logic_vector(ulong_latch); + wr_sig <= '1'; + cnt2_next <= 0; + -- GapList.Bitmap + when 5 => + cnt2_next <= cnt2 + 1; + + data_out <= bitmap_latch_next(cnt2); + wr_sig <= '1'; + + -- Keep Sub-State until Bitmap is written + if (cnt2 /= bitmap_cnt) then + cnt_next <= cnt; + else + -- DONE + stage_next <= SKIP_SUB; + end if; + when others => + null; + end case; + when SID_DATA => + -- Input FIFO Guard + if (empty = '0') then + rd_sig <= '1'; - -- Latch Input for alignment purposes - align_sig_next <= data_in(23 downto 0); - - -- 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; + -- Latch Input for alignment purposes + align_sig_next <= data_in(23 downto 0); + + -- Push Payload + data_out <= data_in_aligned; + wr_sig <= '1'; + + -- Last Payload Word + if (read_cnt & "00" >= 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; + when others => + stage_next <= SKIP_SUB; + end case; end if; when SKIP_SUB => - -- 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 + -- End of Submessage + if ((read_cnt & "00") >= sub_end) then -- Begin parsing of next submessage - stage_next <= RTPS_SUB_HEADER; + stage_next <= RTPS_SUB_HEADER; + -- Valid Submessage End + sub_end_next <= packet_length & "00"; -- Input FIFO Guard elsif (empty = '0') then -- Skip-Read - rd_sig <= '1'; + 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; + -- Valid Submessage End + sub_end_next <= packet_length & "00"; + -- Input FIFO Guard - elsif (empty = '0') then + if (empty = '0') then -- Skip-Read rd_sig <= '1'; end if; + -- NOTE: Exit condition is via the OVERREAD GUARD when others => null; end case; + + -- OVERREAD GUARD + -- Reached End of Packet + if (read_cnt = packet_length) then + -- Force rd_sig low + rd_sig <= '0'; + -- NOTE: This clause is both "legally" taken (e.g. through SKIP_PACKET), or also "illegally" (unexpected) during read sequences (If Packet has less size than expected) + -- Because this clause can also be taken while we are pushing to the Endpoints, we inform possible listening Endpoints of the abrubt ending. + -- Endpoints thus can receive incomplete DATA PAYLOADs that they have to handle accordingly. + -- Notify Endpoints of EOP + last_word_out <= '1'; + -- Continue parsing next + stage_next <= SRC_ADDR_HEADER; + -- Reset packet_length until new packet_length is read (Prevent false positives) + packet_length_next <= (others => '1'); + -- Read outside of Submessage Length + -- XXX: Read from signal we set + -- NOTE: If the Submessage Length is smaller than expected for a particular Submessage, there will be a read from input FIFO while + -- the Submessage Length has been reached and will be caught by this clause. + -- The SKIP_SUB clause prevents a read signal from occuring in this situation, and thus prevents from entering this state. + elsif (rd_sig = '1' and (read_cnt & "00") >= sub_end) then + -- Invalid Submessage Length Field, Skip Packet (see DDSI-RTPS 2.3 Section 8.3.4.1) + stage_next <= SKIP_PACKET; + -- Valid Submessage End + sub_end_next <= packet_length & "00"; + end if; end process; -- Process responsible for counting read words @@ -1045,24 +1285,31 @@ begin if (reset = '1') then stage <= SRC_ADDR_HEADER; ip_addr_type <= INVALID; + packet_length <= (others => '1'); + sub_end <= (others => '1'); 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'); flags <= (others => '0'); - reader_entityid <= (others => '0'); - writer_entityid <= (others => '0'); + src_entityid <= (others => '0'); + dest_entityid <= (others => '0'); user_endpoint <= (others => '0'); numlocators <= (others => '0'); opcode <= (others => '0'); data_header_end <= (others => '0'); - src_guidprefix <= (others => (others => '0')); - seq_nr <= (others => (others => '0')); + long_latch <= (others => '0'); + ulong_latch <= (others => '0'); + bitmap_cnt <= (others => '0'); + bitmap_latch <= (others => (others => '0')); + src_ts <= TIME_INVALID; + src_guidprefix <= GUIDPREFIX_UNKNOWN; + sn_latch_1 <= SEQUENCENUMBER_UNKNOWN; + sn_latch_2 <= SEQUENCENUMBER_UNKNOWN; + sn_latch_3 <= SEQUENCENUMBER_UNKNOWN; cnt <= 0; - is_data <= '0'; + cnt2 <= 0; src_is_reader <= '0'; locator_match <= '0'; builtin_endpoint <= '0'; @@ -1078,16 +1325,23 @@ begin src_addr <= src_addr_next; src_port <= src_port_next; flags <= flags_next; - reader_entityid <= reader_entityid_next; - writer_entityid <= writer_entityid_next; + src_entityid <= src_entityid_next; + dest_entityid <= dest_entityid_next; user_endpoint <= user_endpoint_next; numlocators <= numlocators_next; opcode <= opcode_next; data_header_end <= data_header_end_next; + long_latch <= long_latch_next; + ulong_latch <= ulong_latch_next; + bitmap_cnt <= bitmap_cnt_next; + bitmap_latch <= bitmap_latch_next; + src_ts <= src_ts_next; src_guidprefix <= src_guidprefix_next; - seq_nr <= seq_nr_next; + sn_latch_1 <= sn_latch_1_next; + sn_latch_2 <= sn_latch_2_next; + sn_latch_3 <= sn_latch_3_next; cnt <= cnt_next; - is_data <= is_data_next; + cnt2 <= cnt2_next; src_is_reader <= src_is_reader_next; locator_match <= locator_match_next; builtin_endpoint <= builtin_endpoint_next; diff --git a/src/rtps_package.vhd b/src/rtps_package.vhd index 17d362a..76f2358 100644 --- a/src/rtps_package.vhd +++ b/src/rtps_package.vhd @@ -45,6 +45,7 @@ package rtps_package is constant LOCATOR_KIND_WIDTH : natural := CDR_LONG_WIDTH; constant COUNT_WIDTH : natural := CDR_LONG_WIDTH; constant SUBMESSAGE_DATA_EXTRA_FLAGS_WIDTH : natural := 16; + constant MAX_BITMAP_WIDTH : natural := 256; -- *TYPES DEFINITION* -- Generic Types @@ -52,19 +53,30 @@ package rtps_package is type DOUBLE_WORD_ARRAY is array (0 to 1) of unsigned(WORD_WIDTH-1 downto 0); -- RTPS -- TODO: Define unconstrained WORD_ARRAY and define constrained subtypes - subtype SEQUENCE_NR_TYPE is DOUBLE_WORD_ARRAY; + subtype SEQUENCENUMBER_TYPE is DOUBLE_WORD_ARRAY; subtype DURATION_TYPE is DOUBLE_WORD_ARRAY; subtype TIME_TYPE is DOUBLE_WORD_ARRAY; type GUIDPREFIX_TYPE is array (0 to (GUIDPREFIX_WIDTH/WORD_WIDTH)-1) of std_logic_vector(WORD_WIDTH-1 downto 0); type GUID_TYPE is array (0 to (GUID_WIDTH/WORD_WIDTH)-1) of std_logic_vector(WORD_WIDTH-1 downto 0); + type BITMAP_TYPE is array (0 to (MAX_BITMAP_WIDTH/WORD_WIDTH)-1) of std_logic_vector(0 to WORD_WIDTH-1); -- Helper Function function gen_duration(s,ns : integer) return DURATION_TYPE; -- *PREDEFINED VALUES* constant DEFAULT_IPv4_META_ADDRESS : std_logic_vector(IPv4_ADDRESS_WIDTH-1 downto 0) := x"EFFF0001"; -- Default Multicast Ipv4 Address (239.255.0.1) + constant DURATION_ZERO : DURATION_TYPE := (others => (others => '0')); constant DURATION_INFINITE : DURATION_TYPE := (x"7fffffff", x"ffffffff"); + constant TIME_ZERO : TIME_TYPE := (others => (others => '0')); + constant TIME_INVALID : TIME_TYPE := (others => (others => '1')); + constant TIME_INFINITE : TIME_TYPE := (x"ffffffff", x"fffffffe"); + constant SEQUENCENUMBER_UNKNOWN : SEQUENCENUMBER_TYPE := (x"ffffffff", x"00000000"); constant PROTOCOL_RTPS : std_logic_vector(PROTOCOL_WIDTH-1 downto 0) := x"52545053"; -- 'RTPS' in Ascii code + constant PROTOCOLVERSION_1_0 : std_logic_vector(PROTOCOLVERSION_WIDTH-1 downto 0) := x"0100"; + constant PROTOCOLVERSION_1_1 : std_logic_vector(PROTOCOLVERSION_WIDTH-1 downto 0) := x"0101"; + constant PROTOCOLVERSION_2_0 : std_logic_vector(PROTOCOLVERSION_WIDTH-1 downto 0) := x"0200"; + constant PROTOCOLVERSION_2_1 : std_logic_vector(PROTOCOLVERSION_WIDTH-1 downto 0) := x"0201"; + constant PROTOCOLVERSION_2_2 : std_logic_vector(PROTOCOLVERSION_WIDTH-1 downto 0) := x"0202"; constant PROTOCOLVERSION_2_4 : std_logic_vector(PROTOCOLVERSION_WIDTH-1 downto 0) := x"0204"; constant VENDORID_UNKNOWN : std_logic_vector(VENDORID_WIDTH-1 downto 0) := (others => '0'); constant GUIDPREFIX_UNKNOWN : GUIDPREFIX_TYPE := (others => (others => '0')); @@ -87,6 +99,7 @@ package rtps_package is constant SID_DATA_FRAG : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"16"; -- *SUBMESSAGE FLAG POSITIONS* + -- NOTE: The KEY and NON_STANDARD_PAYOAD Flags for the DATA_FRAG Submessage are off by one (-1) from this positions constant SUBMESSAGE_ENDIAN_FLAG_POS : natural := 0; constant SUBMESSAGE_FINAL_FLAG_POS : natural := 1; constant SUBMESSAGE_INLINE_QOS_FLAG_POS : natural := 1; @@ -140,7 +153,7 @@ package rtps_package is constant PID_BUILTIN_ENDPOINT_SET : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0058"; constant PID_BUILTIN_ENDPOINT_QOS : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0077"; constant PID_PROPERTY_LIST : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0059"; - constant PID_TYPE_MAX_SIZE_SERIALIZED : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0060"; + constant PID_DATA_MAX_SIZE_SERIALIZED : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0060"; constant PID_ENTITY_NAME : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0062"; constant PID_ENDPOINT_GUID : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"005a"; -- INLINE-QOS ONLY @@ -186,13 +199,14 @@ package rtps_package is constant ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER: std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000200c2"); constant ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER: std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000200c7"); -- *ENTITY KIND* (Last Byte of Entity ID) - subtype ENTITY_KIND_H_RANGE is natural range 1 downto 0; - subtype ENTITY_KIND_L_RANGE is natural range 7 downto 2; + subtype ENTITY_KIND_H_RANGE is natural range 7 downto 6; + subtype ENTITY_KIND_L_RANGE is natural range 5 downto 0; --FOLLOWING MAP TO ENTITY_KIND_H constant USER_DEFINED_ENTITY : std_logic_vector(ENTITY_KIND_H_RANGE) := "00"; constant BUILT_IN_ENTITY : std_logic_vector(ENTITY_KIND_H_RANGE) := "11"; constant VENDOR_SPECIFIC_ENTITY : std_logic_vector(ENTITY_KIND_H_RANGE) := "01"; --FOLLOWING MAP TO ENTITY_KIND_L + constant UNKNOWN_KIND : std_logic_vector(ENTITY_KIND_L_RANGE) := (others => '0'); constant WRITER_WITH_KEY : std_logic_vector(ENTITY_KIND_L_RANGE) := "000010"; constant WRITER_NO_KEY : std_logic_vector(ENTITY_KIND_L_RANGE) := "000011"; constant READER_NO_KEY : std_logic_vector(ENTITY_KIND_L_RANGE) := "000100"; diff --git a/syn/DE10-Nano/top.qsf b/syn/DE10-Nano/top.qsf index b923cad..56b525d 100644 --- a/syn/DE10-Nano/top.qsf +++ b/syn/DE10-Nano/top.qsf @@ -39,7 +39,7 @@ set_global_assignment -name FAMILY "Cyclone V" set_global_assignment -name DEVICE 5CGXFC7C7F23C8 -set_global_assignment -name TOP_LEVEL_ENTITY rtps_builtin_endpoint +set_global_assignment -name TOP_LEVEL_ENTITY rtps_handler set_global_assignment -name ORIGINAL_QUARTUS_VERSION 20.1.0 set_global_assignment -name PROJECT_CREATION_TIME_DATE "13:33:09 NOVEMBER 02, 2020" set_global_assignment -name LAST_QUARTUS_VERSION "20.1.0 Lite Edition"