From aaae545c8bab41aa75999101efc947b1ea2dd91b Mon Sep 17 00:00:00 2001 From: Greek64 Date: Tue, 22 Sep 2020 21:04:29 +0200 Subject: [PATCH] * Partly implemented rtps_builtin_endpoint * Modify rtps_handler * Modify rtps_package --- src/rtps_builtin_endpoint.vhd | 1058 +++++++++++++++++++++++++++++++++ src/rtps_handler.vhd | 114 ++-- src/rtps_package.vhd | 194 +++++- 3 files changed, 1311 insertions(+), 55 deletions(-) create mode 100644 src/rtps_builtin_endpoint.vhd diff --git a/src/rtps_builtin_endpoint.vhd b/src/rtps_builtin_endpoint.vhd new file mode 100644 index 0000000..92c6cb8 --- /dev/null +++ b/src/rtps_builtin_endpoint.vhd @@ -0,0 +1,1058 @@ +library ieee; +use ieee.std_logic_1164.all; +use ieee.numeric_std.all; + +use work.math_pkg.all; +use work.rtps_package.all; + +-- Checksum has to be checked before + +entity rtps_handler is + generic ( + DOMAIN_ID : integer := 0; + DOMAIN_TAG : std_logic_vector(0 to (256*8)-1) := (others => '0') + ) + 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 + + --*****COMPONENT DECLARATION****** + + --*****CONSTANT DECLARATION***** + + --*****TYPE DECLARATION***** + -- FSM states. Explained below in detail + type STAGE_TYPE is (TODO); + type MEM_STAGE_TYPE is (TODO); + type MESSAGE_TYPE_TYPE is (PDP, EDP, MESSAGE); + type MEM_OPCODE_TYPE is (NOP, SEARCH_PARTICIPANT, SEARCH_ENDPOINT); + + + --*****SIGNAL DECLARATION***** + -- FSM state + signal stage, stage_next : STAGE_TYPE := SRC_ADDR_HEADER; + signal mem_stage, mem_stage_next : MEM_STAGE_TYPE := TODO; + -- 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 Word aligned total packet length + signal packet_length, packet_length_next : unsigned(13 downto 0) := (others => '0'); + signal opcode, opcode_next : std_logic_vector(7 downto 0) := (others => '0'); + signal flags, flags_next : std_logic_vector(7 downto 0) := (others => '0'); + signal src_port, src_port_next : std_logic_vector(15 downto 0) := (others => '0'); + signal src_addr, src_addr_next : std_logic_vector(31 downto 0) := (others => '0'); + signal addr_latch_1, addr_latch_1_next, addr_latch_2, addr_latch_2_next : std_logic_vector(31 downto 0) := (others => '0'); + signal port_latch_1, port_latch_1_next, port_latch_2, port_latch_2_next : std_logic_vector(15 downto 0) := (others => '0'); + signal src_entityid, src_entityid_next : std_logic_vector(31 downto 0) := (others => '0'); + signal dest_entityid, dest_entityid_next : std_logic_vector(31 downto 0) := (others => '0'); + signal parameter_end, parameter_end_next : unsigned(13 downto 0) := (others => '0'); + signal message_type, message_type_next : MESSAGE_TYPE_TYPE := PDP; + signal data_in_swapped : std_logic_vector(31 downto 0) := (others => '0'); + signal string_length, string_length_next : unsigned(31 downto 0) := (others => '0'); + --TODO: Always reset before COMPARE_STRING stage + signal compare_length, compare_length_next : unsigned(31 downto 0) := (others => '0'); + signal endpoint_match, endpoint_match_next : std_logic_vector(MAX_ENDPOINTS-1 downto 0) := (others => '0'); + signal is_requested, is_requested_next : std_logic := '0'; + signal lease_duration, lease_duration_next : DURATION_TYPE := (others => (others => '0')); + signal lifespan_duration, lifespan_duration_next : DURATION_TYPE := (others => (others => '0')); + signal addr_latch_index, addr_latch_index_next : std_logic := '0'; + -- General Purpose counter + -- TODO: Constrain range + signal cnt, cnt_next : integer := 0; + signal expects_inline_qos, expects_inline_qos_next : std_logic := '0'; + signal guid, guid_next : GUID_ARRAY_TYPE := (others => (others => '0')); + signal start_mem_op, mem_op_busy, mem_op_done : std_logic := '0'; + signal mem_opcode : MEM_OPCODE_TYPE := NOP; + signal participant_message_best_effort, participant_message_best_effort_next : std_logic := '0'; + + --*****ALIAS DEFINATION***** + alias header_opcode : std_logic_vector(7 downto 0) is data_in(31 downto 24); + alias header_flags : std_logic_vector(7 downto 0) is data_in(23 downto 16); + alias header_udp_port : std_logic_vector(15 downto 0) is data_in(15 downto 0); + alias parameter_id : std_logic_vector(15 downto 0) is data_in(31 downto 16); + alias parameter_length : std_logic_vector(15 downto 0) is data_in(15 downto 0); + alias representation_id : std_logic_vector(15 downto 0) is data_in(31 downto 16); + alias representation_options : std_logic_vector(15 downto 0) is data_in(15 downto 0); + -- RTPS SUBMESSAGE FLAGS + alias endian_flag : std_logic is flags(0); + alias endian_flag_next : std_logic is flags_next(0); + alias qos_flag : std_logic is flags(1); + alias qos_flag_next : std_logic is flags_next(1); + alias data_flag : std_logic is flags(2); + alias key_flag : std_logic is flags(3); + alias payload_flag : std_logic is header_flags(4); + alias must_undersand : std_logic is parameter_id(14); + + --*****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; + + +begin + + rd <= rd_sig; + + endian_swap_prc: process(all) + begin + data_in_swapped <= data_in; + if (endian_flag = '1') then + for i in 0 to 3 loop + data_in_swapped(i*8+8-1 downto i*8) <= data((3-i)*8+8-1 downto (3-i)*8); + end loop; + end if; + end process; + + parse_prc: process(all) + variable tmp_mask : std_logic_vector(31 downto 0):= (others => '0'); + begin + --DEFAULT + stage_next <= stage; + packet_length_next <= packet_length; + rd_sig <= '0'; + reset_read_cnt <= '0'; + opcode_next <= opcode; + flags_next <= flags; + src_port_next <= src_port; + src_addr_next <= src_addr; + src_entityid_next <= src_entityid; + dest_entityid_next <= dest_entityid; + parameter_end_next <= parameter_end; + message_type_next <= message_type; + string_length_next <= string_length; + endpoint_match_next <= endpoint_match; + is_requested_next <= is_requested; + lease_duration_next <= lease_duration; + lifespan_duration_next <= lifespan_duration; + addr_latch_1_next <= addr_latch_1; + addr_latch_2_next <= addr_latch_2; + port_latch_1_next <= port_latch_1; + port_latch_2_next <= port_latch_2; + addr_latch_index_next <= addr_latch_index; + cnt_next <= cnt; + expects_inline_qos_next <= expects_inline_qos; + mem_opcode <= NOP; + start_mem_op <= '0'; + participant_message_best_effort_next <= participant_message_best_effort; + + -- TODO: Reset Latches + + case(stage) is + -- Initial/Idle State + when INIT => + if (empty = '0') then + rd_sig <= '1'; + -- Latch Packet Length + packet_length_next <= unsigned(data_in(13 downto 0)); + -- Reset Word Counter + reset_read_cnt <= '1'; + -- Next Stage + stage_next <= PACKET_HEADER; + end if; + when PACKET_HEADER => + if (empty = '0') then + rd_sig <= '1'; + -- Latch Opcode + opcode_next <= header_opcode; + -- Latch Flags + flags_next <= header_flags; + -- Latch Source UDP Port + src_port_next <= header_udp_port; + -- Next Stage + stage_next <= PACKET_SRC_ADDR; + -- SANITY CHECK + -- Skip Packet if non-standard Payload + if(header_opcode = SID_DATA and payload_flag = '1') then + stage_next <= SKIP_PACKET; + end if; + end if; + when PACKET_SRC_ADDR => + if (empty = '0') then + rd_sig <= '1'; + -- Latch Source IP Address + src_addr_next <= data_in; + -- Next Stage + stage_next <= PACKET_SRC_ENTITYID; + end if; + when PACKET_SRC_ENTITYID => + if (empty = '0') then + rd_sig <= '1'; + -- Latch Source Entity ID + src_entityid_next <= data_in; + -- Next Stage + stage_next <= PACKET_DEST_ENTITYID; + end if; + when PACKET_DEST_ENTITYID => + if (empty = '0') then + rd_sig <= '1'; + + -- DEFAULT STAGE + stage_next <= SKIP_PACKET; + + -- *Check Dest Entity ID* + case (data_in) is + when ENTITYID_UNKNOWN => + -- Wildcard Destination + -- Content has to be determined through src + stage_next <= CHECK_SRC_ENTITYID; + when ENTITYID_SPDP_BUILTIN_PARTICIPANT_DETECTOR => + -- Only DATA relevant + if (opcode = SID_DATA) then + -- Packet Contains Participant Data + message_type_next <= PDP; + stage_next <= PROCESS_DATA; + end if; + when ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER => + -- SANITY CHECK: Ignore if no Writers + -- Only ACKNACKs are relevant + if (NUM_WRITERS /= 0 and opcode = SID_ACKNACK) then + message_type_next <= EDP; + stage_next <= TODO; --ACKNACK Processing + end if; + when ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR => + -- SANITY CHECK: Ignore if no Readers + if (NUM_READERS /= 0) then + -- Only HEARTBEATs and DATA are relevant (GAPs irrelevant, since depth is 1) + case (opcode) is + when SID_HEARTBEAT => + message_type_next <= EDP; + stage_next <= TODO; -- HEARTBEAT Processing + when SID_DATA => + message_type_next <= EDP; + stage_next <= PROCESS_DATA; -- DATA Processing + -- QoS is publisher-offered + is_requested_next <= '0'; + -- Reset Endpoint Match (ALL READERS) + endpoint_match_next <= (others => '0'); + endpoint_match_next(MAX_ENDPOINTS-1 downto MAX_ENDPOINTS-NUM_READERS) <= (others => '1'); + end case; + end if; + when ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER => + -- SANITY CHECK: Ignore if no Readers + -- Only ACKNACKs are relevant + if (NUM_READERS /= 0 and opcode = SID_ACKNACK) then + message_type_next <= EDP; + stage_next <= TODO; --ACKNACK Processing + end if; + when ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR => + -- SANITY CHECK: Ignore if no Writers + if (NUM_WRITERS /= 0) then + -- Only HEARTBEATs and DATA are relevant (GAPs irrelevant, since depth is 1) + case (opcode) is + when SID_HEARTBEAT => + message_type_next <= EDP; + stage_next <= TODO; -- HEARTBEAT Processing + when SID_DATA => + message_type_next <= EDP; + stage_next <= PROCESS_DATA; -- DATA Processing + -- QoS is subscriber-requested + is_requested_next <= '1'; + -- Reset Endpoint Match (ALL WRITERS) + endpoint_match_next <= (others => '0'); + endpoint_match_next(NUM_WRITERS-1 downto 0) <= (others => '1'); + end case; + end if; + when ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER => + -- Only ACKNACKs are relevant + if (opcode = SID_ACKNACK) then + message_type_next <= MESSAGE; + stage_next <= TODO; --ACKNACK Processing + end if; + when ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER => + -- Only HEARTBEATs and DATA are relevant (GAPs irrelevant, since depth is 1) + case (opcode) is + when SID_HEARTBEAT => + message_type_next <= MESSAGE; + stage_next <= TODO; -- HEARTBEAT Processing + when SID_DATA => + message_type_next <= MESSAGE; + stage_next <= PROCESS_DATA; -- DATA Processing + end case; + when others => + null; + end case; + end if; + when CHECK_SRC_ENTITYID => + -- DEFAULT STAGE + stage_next <= SKIP_PACKET; + + -- *Check Dest Entity ID* + case (src_entityid) is + when ENTITYID_SPDP_BUILTIN_PARTICIPANT_ANNOUNCER => + -- Only DATA relevant + if (opcode = SID_DATA) then + -- Packet Contains Participant Data + message_type_next <= PDP; + stage_next <= PROCESS_DATA; + end if; + when ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR => + -- SANITY CHECK: Ignore if no Writers + -- Only ACKNACKs are relevant + if (NUM_WRITERS /= 0 and opcode = SID_ACKNACK) then + message_type_next <= EDP; + stage_next <= TODO; --ACKNACK Processing + end if; + when ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER => + -- SANITY CHECK: Ignore if no Readers + if (NUM_READERS /= 0) then + -- Only HEARTBEATs and DATA are relevant (GAPs irrelevant, since depth is 1) + case (opcode) is + when SID_HEARTBEAT => + message_type_next <= EDP; + stage_next <= TODO; -- HEARTBEAT Processing + when SID_DATA => + message_type_next <= EDP; + stage_next <= PROCESS_DATA; -- DATA Processing + -- QoS is publisher-offered + is_requested_next <= '0'; + -- Reset Endpoint Match (ALL READERS) + endpoint_match_next <= (others => '0'); + endpoint_match_next(MAX_ENDPOINTS-1 downto MAX_ENDPOINTS-NUM_READERS) <= (others => '1'); + end case; + end if; + when ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR => + -- SANITY CHECK: Ignore if no Readers + -- Only ACKNACKs are relevant + if (NUM_READERS /= 0 and opcode = SID_ACKNACK) then + message_type_next <= EDP; + stage_next <= TODO; --ACKNACK Processing + end if; + when ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER => + -- SANITY CHECK: Ignore if no Writers + if (NUM_WRITERS /= 0) then + -- Only HEARTBEATs and DATA are relevant (GAPs irrelevant, since depth is 1) + case (opcode) is + when SID_HEARTBEAT => + message_type_next <= EDP; + stage_next <= TODO; -- HEARTBEAT Processing + when SID_DATA => + message_type_next <= EDP; + stage_next <= PROCESS_DATA; -- DATA Processing + -- QoS is subscriber-requested + is_requested_next <= '1'; + -- Reset Endpoint Match (ALL WRITERS) + endpoint_match_next <= (others => '0'); + endpoint_match_next(NUM_WRITERS-1 downto 0) <= (others => '1'); + end case; + end if; + when ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER => + -- Only ACKNACKs are relevant + if (opcode = SID_ACKNACK) then + message_type_next <= MESSAGE; + stage_next <= TODO; --ACKNACK Processing + end if; + when ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER => + -- Only HEARTBEATs and DATA are relevant (GAPs irrelevant, since depth is 1) + case (opcode) is + when SID_HEARTBEAT => + message_type_next <= MESSAGE; + stage_next <= TODO; -- HEARTBEAT Processing + when SID_DATA => + message_type_next <= MESSAGE; + stage_next <= PROCESS_DATA; -- DATA Processing + end case; + when others => + null; + end case; + when PROCESS_DATA => + -- Data Message contains inline QoS + if (qos_flag = '1') then + stage_next <= PROCESS_PL; + -- Data Message contains Payload + elsif (data_flag = '1') then + -- Process Payload Header + if (empty = '0') then + rd_sig <= '1'; + -- DEFAULT Stage Next + stage_next <= SKIP_PACKET; + + -- Process Payload Header + case(representation_id) is + when PL_CDR_BE => + endian_flag_next <= '0'; + stage_next <= PROCESS_PL; + when PL_CDR_LE => + endian_flag_next <= '1'; + stage_next <= PROCESS_PL; + when CDR_BE => + if (message_type = MESSAGE) then + endian_flag_next <= '0'; + stage_next <= PROCESS_MESSAGE; + end if; + when CDR_LE => + if (message_type = MESSAGE) then + endian_flag_next <= '1'; + stage_next <= PROCESS_MESSAGE; + end if; + when others => + null; + end case; + end if; + end if; + when PROCESS_PL => + if (empty = '0') then + rd_sig <= '1'; + -- Latch Parameter End + parameter_end_next <= read_cnt + unsigned(normalize_length(endian_swap(endian_flag,parameter_length))); + -- DEFAULT + stage_next <= SKIP_PARAMETER; + -- TODO: Ignore all inline QoS? + + case (parameter_id) is + when PID_TOPIC_NAME => + -- Topic Name only relevant for EDP + if(qos_flag = '0' and message_type = EDP) then + stage_next <= LATCH_STRING_LENGTH; + end if; + when PID_TYPE_NAME => + -- Type Name only relevant for EDP + if(qos_flag = '0' and message_type = EDP) then + stage_next <= LATCH_STRING_LENGTH; + end if; + when PID_USER_DATA => + -- Ignore + null; + when PID_GROUP_DATA => + -- Ignore + null; + when PID_TOPIC_DATA => + -- Ignore + null; + when PID_DURABILITY => + if(qos_flag = '0' and message_type = EDP) then + stage_next <= RXO_DURABILITY; + end if; + when PID_DURABILITY_SERVICE => + -- Ignore + when PID_DEADLINE => + if(qos_flag = '0' and message_type = EDP) then + stage_next <= RXO_DEADLINE_1; + end if; + when PID_LATENCY_BUDGET => + -- Ignore + when PID_LIVELINESS => + if(qos_flag = '0' and message_type = EDP) then + stage_next <= RXO_LIVELINESS; + end if; + when PID_RELIABILITY => + if(qos_flag = '0' and message_type = EDP) then + stage_next <= RXO_RELIABILITY; + end if; + when PID_LIFESPAN => + if(qos_flag = '0' and message_type = EDP) then + stage_next <= LATCH_LIFESPAN_1; + end if; + when PID_DESTINATION_ORDER => + if(qos_flag = '0' and message_type = EDP) then + stage_next <= RXO_DESTINATION_ORDER; + end if; + when PID_HISTORY => + -- Ignore + when PID_RESOURCE_LIMITS => + -- Ignore + when PID_OWNERSHIP => + if(qos_flag = '0' and message_type = EDP) then + stage_next <= RXO_OWNERSHIP; + end if; + when PID_OWNERSHIP_STRENGTH => + -- Ignore + when PID_PRESENTATION => + if(qos_flag = '0' and message_type = EDP) then + stage_next <= RXO_PRESENTATION; + end if; + when PID_PARTITION => + if(qos_flag = '0' and message_type = EDP) then + stage_next <= RXO_PARTITION; + end if; + when PID_TIME_BASED_FILTER => + -- Ignore + when PID_TRANSPORT_PRIORITY => + -- Ignore + when PID_DOMAIN_ID => + if(qos_flag = '0' and message_type = PDP) then + stage_next <= MATCH_DOMAIN_ID; + end if; + when PID_DOMAIN_TAG => + if(qos_flag = '0' and message_type = PDP) then + stage_next <= LATCH_STRING_LENGTH; + end if; + when PID_PROTOCOL_VERSION => + if(qos_flag = '0' and message_type = PDP) then + stage_next <= MATCH_PROTOCOL_VERSION; + end if; + when PID_VENDORID => + -- Ignore + when PID_UNICAST_LOCATOR => + if(qos_flag = '0' and message_type = EDP) then + stage_next <= LATCH_LOCATOR; + addr_latch_index_next <= '0'; + -- Initialise counter + cnt_next <= 1; + end if; + when PID_MULTICAST_LOCATOR => + if(qos_flag = '0' and message_type = EDP) then + stage_next <= LATCH_LOCATOR; + addr_latch_index_next <= '0'; + -- Initialise counter + cnt_next <= 1; + end if; + when PID_DEFAULT_UNICAST_LOCATOR => + if(qos_flag = '0' and message_type = DPD) then + stage_next <= LATCH_LOCATOR; + addr_latch_index_next <= '0'; + -- Initialise counter + cnt_next <= 1; + end if; + when PID_DEFAULT_MULTICAST_LOCATOR => + if(qos_flag = '0' and message_type = DPD) then + stage_next <= LATCH_LOCATOR; + addr_latch_index_next <= '0'; + -- Initialise counter + cnt_next <= 1; + end if; + when PID_METATRAFFIC_UNICAST_LOCATOR => + if(qos_flag = '0' and message_type = DPD) then + stage_next <= LATCH_LOCATOR; + addr_latch_index_next <= '1'; + -- Initialise counter + cnt_next <= 1; + end if; + when PID_METATRAFFIC_MULTICAST_LOCATOR => + if(qos_flag = '0' and message_type = DPD) then + stage_next <= LATCH_LOCATOR; + addr_latch_index_next <= '1'; + -- Initialise counter + cnt_next <= 1; + end if; + when PID_EXPECTS_INLINE_QOS => + stage_next <= LATCH_EXPECTS_INLINE_QOS; + when PID_PARTICIPANT_MANUAL_LIVELINESS_COUNT => + -- Ignore + when PID_PARTICIPANT_LEASE_DURATION => + if(qos_flag = '0' and message_type = DPD) then + stage_next <= LATCH_LEASE_DURATION_1; + end if; + when PID_CONTENT_FILTER_PROPERTY => + -- Ignore + when PID_PARTICIPANT_GUID => + if(qos_flag = '0' and message_type = DPD) then + stage_next <= LATCH_GUID; + -- Initialise counter + cnt_next <= 0; + end if; + when PID_GROUP_GUID => + -- Ignore + when PID_BUILTIN_ENDPOINT_SET => + if(qos_flag = '0' and message_type = DPD) then + stage_next <= CHECK_REMOTE_BUILTIN_ENDPOINTS; + end if; + when PID_BUILTIN_ENDPOINT_QOS => + if(qos_flag = '0' and message_type = DPD) then + stage_next <= LATCH_BUILTIN_ENDPOINT_QOS; + end if; + when PID_PROPERTY_LIST => + -- Ignore + when PID_TYPE_MAX_SIZE_SERIALIZED => + -- Ignore + when PID_ENTITY_NAME => + -- Ignore + when PID_ENDPOINT_GUID => + if(qos_flag = '0' and message_type = EPD) then + stage_next <= LATCH_GUID; + -- Initialise counter + cnt_next <= 0; + end if; + when PID_CONTENT_FILTER_INFO => + -- Ignore + when PID_COHERENT_SET => + -- Ignore + when PID_DIRECTED_WRITE => + -- Ignore + when PID_ORIGINAL_WRITER_INFO => + -- Ignore + when PID_GROUP_COHERENT_SET => + -- Ignore + when PID_GROUP_SEQ_NUM => + -- Ignore + when PID_WRITER_GROUP_INFO => + -- Ignore + when PID_SECURE_WRITER_GROUP_INFO => + -- Ignore + when PID_KEY_HASH => + -- Ignore + when PID_STATUS_INFO => + -- Ignore + when PID_EXTENDED => + -- TODO + when PID_PAD => + -- Ignore + when PID_IGNORE => + -- Ignore + when PID_SENTINEL => + -- If Processing in-line QoS until now + if(qos_flag = '1') then + qos_flag_next <= '0'; + stage_next <= PROCESS_DATA; + end if; + -- TODO: End Condition + when PID_LIST_END => + -- TODO + when others => + -- If MUST_UNDERSTAND Flag is set, we have incompatible communication. Drop Packet + if (must_undersand = '1') then + stage_next <= SKIP_PACKET; + -- Else skip Uknown Parameter + else + stage_next <= SKIP_PARAMETER; + end if; + end case; + end if; + when LATCH_STRING_LENGTH => + if (empty = '0') then + rd_sig <= '1'; + string_length_next <= unsigned(data_in_swapped); + stage_next <= COMPARE_STRING; + compare_length_next <= (others => '0'); + end if; + when COMPARE_STRING => + if (empty = '0') then + rd_sig <= '1'; + -- Increment compare position counter + compare_length_next <= compare_length + to_unsigned(32, compare_length'length); + -- Compare Strings + if (message_type = EDP) then + for i in 0 to ENDPOINT_TOPIC'length-1 loop + if (data_in /= ENDPOINT_TOPIC(i)(to_integer(compare_length) to to_integer(compare_length)+7)) then + endpoint_match_next(i) <= '0'; + end if; + end loop; + elsif (message_type = DPD) then + if (data_in /= DOMAIN_TAG(to_integer(compare_length) to to_integer(compare_length)+7)) then + endpoint_match_next(i) <= '0'; + end if; + end if; + -- End of String (Exit Condition) + -- NOTE: This assumes that the Parameter is padded with zeroes + -- TODO: XTypes 7.4.1.1 requires padding bytes for "PLAIN_CDR" to be zero, but does not specify the value for "PL_CDR" + if (compare_length >= string_length) then + stage_next <= SKIP_PARAMETER; + end if; + end if; + when RXO_DURABILITY => + if (empty = '0') then + rd_sig <= '1'; + + -- Check QoS Compatibility + for i in 0 to ENDPOINT_TOPIC'length-1 loop + -- data-in is Requested + if (is_requested = '1') then + if (data_in_swapped > ENDPOINT_DURABILITY(i)) then + endpoint_match_next(i) <= '0'; + end if; + -- data-in is Offered + else + if (data_in_swapped < ENDPOINT_DURABILITY(i)) then + endpoint_match_next(i) <= '0'; + end if; + end if; + end loop; + -- Next Stage + stage_next <= SKIP_PARAMETER; + end if; + when RXO_DEADLINE_1 => + if (empty = '0') then + rd_sig <= '1'; + + -- Check QoS Compatibility + for i in 0 to ENDPOINT_TOPIC'length-1 loop + -- data-in is Requested + if (is_requested = '1') then + if (data_in_swapped < ENDPOINT_DEADLINE(i)(1)) then + endpoint_match_next(i) <= '0'; + end if; + -- data-in is Offered + else + if (data_in_swapped > ENDPOINT_DEADLINE(i)(1)) then + endpoint_match_next(i) <= '0'; + end if; + end if; + end loop; + -- Next Stage + stage_next <= RXO_DEADLINE_2; + end if; + when RXO_DEADLINE_2 => + if (empty = '0') then + rd_sig <= '1'; + + -- Check QoS Compatibility + for i in 0 to ENDPOINT_TOPIC'length-1 loop + -- data-in is Requested + if (is_requested = '1') then + if (data_in_swapped < ENDPOINT_DEADLINE(i)(0)) then + endpoint_match_next(i) <= '0'; + end if; + -- data-in is Offered + else + if (data_in_swapped > ENDPOINT_DEADLINE(i)(0)) then + endpoint_match_next(i) <= '0'; + end if; + end if; + end loop; + -- Next Stage + stage_next <= SKIP_PARAMETER; + end if; + when RXO_LIVELINESS => + if (empty = '0') then + rd_sig <= '1'; + + -- Check QoS Compatibility + for i in 0 to ENDPOINT_TOPIC'length-1 loop + -- data-in is Requested + if (is_requested = '1') then + if (data_in_swapped > ENDPOINT_LIVELINESS(i)) then + endpoint_match_next(i) <= '0'; + end if; + -- data-in is Offered + else + if (data_in_swapped < ENDPOINT_LIVELINESS(i)) then + endpoint_match_next(i) <= '0'; + end if; + end if; + end loop; + -- Next Stage + stage_next <= LATCH_LEASE_DURATION_1; + end if; + when LATCH_LEASE_DURATION_1 => + if (empty = '0') then + rd_sig <= '1'; + -- Latch Lease Duration + lease_duration_next(1) <= data_in_swapped; + -- Next Stage + stage_next <= LATCH_LEASE_DURATION_2; + end if; + when LATCH_LEASE_DURATION_2 => + if (empty = '0') then + rd_sig <= '1'; + -- Latch Lease Duration + lease_duration_next(0) <= data_in_swapped; + -- Next Stage + stage_next <= SKIP_PARAMETER; + end if; + when RXO_RELIABILITY => + if (empty = '0') then + rd_sig <= '1'; + + -- Check QoS Compatibility + for i in 0 to ENDPOINT_TOPIC'length-1 loop + -- data-in is Requested + if (is_requested = '1') then + if (data_in_swapped > ENDPOINT_RELIABILITY(i)) then + endpoint_match_next(i) <= '0'; + end if; + -- data-in is Offered + else + if (data_in_swapped < ENDPOINT_RELIABILITY(i)) then + endpoint_match_next(i) <= '0'; + end if; + end if; + end loop; + -- Next Stage + -- NOTE: The max_blocking_time value is ignored + stage_next <= SKIP_PARAMETER; + end if; + when LATCH_LIFESPAN_1 => + if (empty = '0') then + rd_sig <= '1'; + -- Latch Lease Duration + lifespan_duration_next(1) <= data_in_swapped; + -- Next Stage + stage_next <= LATCH_LIFESPAN_2; + end if; + when LATCH_LIFESPAN_2 => + if (empty = '0') then + rd_sig <= '1'; + -- Latch Lease Duration + lifespan_duration_next(0) <= data_in_swapped; + -- Next Stage + stage_next <= SKIP_PARAMETER; + end if; + when RXO_DESTINATION_ORDER => + if (empty = '0') then + rd_sig <= '1'; + + -- Check QoS Compatibility + for i in 0 to ENDPOINT_TOPIC'length-1 loop + -- data-in is Requested + if (is_requested = '1') then + if (data_in_swapped > ENDPOINT_DESTINATION_ORDER(i)) then + endpoint_match_next(i) <= '0'; + end if; + -- data-in is Offered + else + if (data_in_swapped < ENDPOINT_DESTINATION_ORDER(i)) then + endpoint_match_next(i) <= '0'; + end if; + end if; + end loop; + -- Next Stage + stage_next <= SKIP_PARAMETER; + end if; + when RXO_OWNERSHIP => + if (empty = '0') then + rd_sig <= '1'; + -- Check QoS Compatibility + if (data_in_swapped /= (data_in_swapped'reverse_range => '0')) then + endpoint_match_next <= (others => '0'); + end if; + -- Next Stage + stage_next <= SKIP_PARAMETER; + end if; + when RXO_PRESENTATION_1 => + if (empty = '0') then + rd_sig <= '1'; + + -- Check QoS Compatibility + for i in 0 to ENDPOINT_TOPIC'length-1 loop + -- data-in is Requested + if (is_requested = '1') then + if (data_in_swapped > ENDPOINT_PRESENTATION(i)) then + endpoint_match_next(i) <= '0'; + end if; + -- data-in is Offered + else + if (data_in_swapped < ENDPOINT_PRESENTATION(i)) then + endpoint_match_next(i) <= '0'; + end if; + end if; + end loop; + -- Next Stage + stage_next <= RXO_PRESENTATION_2; + end if; + when RXO_PRESENTATION_2 => + if (empty = '0') then + rd_sig <= '1'; + + -- Check QoS Compatibility + for i in 0 to ENDPOINT_TOPIC'length-1 loop + -- data-in is Requested + if (is_requested = '1') then + if (data_in(23) = '1' and ENDPOINT_COHERENT_ACCESS(i) = '0') then + endpoint_match_next(i) <= '0'; + end if; + if (data_in(15) = '1' and ENDPOINT_ORDERED_ACCESS(i) = '0') then + endpoint_match_next(i) <= '0'; + end if; + end if; + end loop; + -- Next Stage + stage_next <= SKIP_PARAMETER; + end if; + when RXO_PARTITION => + if (empty = '0') then + rd_sig <= '1'; + + --TODO: Implement full partition Name support? + -- NOTE: Endpoints are only matched against the default empty partition. + + -- Check QoS Compatibility + if (data_in_swapped /= (data_in_swapped'reverse_range => '0')) then + endpoint_match_next <= (others => '0'); + end if; + + -- Next Stage + stage_next <= SKIP_PARAMETER; + end if; + when MATCH_DOMAIN_ID => + if (empty = '0') then + rd_sig <= '1'; + -- DEAFULT Stage + stage_next <= SKIP_PARAMETER; + -- MATCH DOMAIN ID + if (data_in_swapped /= std_logic_vector(to_unsigned(DOMAIN_ID,data_in_swapped'length))) then + -- Drop Packet + stage_next <= SKIP_PACKET; + end if; + end if; + when MATCH_PROTOCOL_VERSION => + if (empty = '0') then + rd_sig <= '1'; + -- DEAFULT Stage + stage_next <= SKIP_PARAMETER; + -- If RTPS Protocol Major Version is not 2, skip packet + if(data_in(31 downto 24) /= PROTOCOLVERSION_2_4(15 downto 8)) then + -- Drop Packet + stage_next <= SKIP_PACKET; + end if; + end if; + when LATCH_LOCATOR => + if (empty = '0') then + rd_sig <= '1'; + -- Increment Counter (Default) + cnt_next <= cnt + 1; + -- Locator Kind + if (cnt = 1) then + -- Check if UDPv4 Locator + if (data_in_swapped /= LOCATOR_KIND_UDPv4) then + stage_next <= SKIP_PARAMETER; + end if; + -- Locator Port + elsif (cnt = 2) then + -- Latch Source Port + if (addr_latch_index = '0') then + port_latch_1 <= data_in_swapped(port_latch_1'length-1 downto 0); + else + port_latch_2 <= data_in_swapped(port_latch_2'length-1 downto 0); + end if; + -- Locator Addr (IPv4) + elsif (cnt = 6) then + -- Latch Src Addr + if (addr_latch_index = '0') then + addr_latch_1 <= data_in_swapped; + else + addr_latch_2 <= data_in_swapped; + end if; + -- Exit Stage + stage_next <= SKIP_PARAMETER; + end if; + end if; + when LATCH_EXPECTS_INLINE_QOS => + if (empty = '0') then + rd_sig <= '1'; + + -- Latch 'expectsInlineQoS' + expects_inline_qos_next <= data_in(24); + stage_next <= SKIP_PARAMETER; + end if; + when LATCH_GUID => + -- NOTE: Checking here also for 'mem_op_done' prevents updating the GUID while the mem_op state machine could + -- potentially be using it. + if (empty = '0' and mem_op_done = '1') then + rd_sig <= '1'; + -- Increment Counter (Default) + cnt_next <= cnt + 1; + + -- Latch GUID + guid(cnt) <= data_in; + + -- Exit Condition + if (cnt = 3) then + if (message_type = PDP) then + mem_opcode <= SEARCH_PARTICIPANT; + start_mem_op <= '1'; + elsif (message_type = EDP) then + mem_opcode <= SEARCH_ENDPOINT; + start_mem_op <= '1'; + end if; + stage_next <= SKIP_PARAMETER; + end if; + end if; + when CHECK_REMOTE_BUILTIN_ENDPOINTS => + if (empty = '0') then + rd_sig <= '1'; + + -- DEFAULT Next Stage + stage_next <= SKIP_PARAMETER; + + -- SANITY CHECK (Builtin Participant and Participant Message Endpoints have to be always available) + if ( data_in_swapped(DISC_BUILTIN_ENDPOINT_PARTICIPANT_ANNOUNCER) = '0' or + data_in_swapped(DISC_BUILTIN_ENDPOINT_PARTICIPANT_DETECTOR) = '0' or + data_in_swapped(BUILTIN_ENDPOINT_PARTICIPANT_MESSAGE_DATA_WRITER) = '0' or + data_in_swapped(BUILTIN_ENDPOINT_PARTICIPANT_MESSAGE_DATA_READER) = '0') then + stage_next <= SKIP_PACKET; + end if; + -- Check for necessary Builtin Endpoints for Readers + if (NUM_READERS > 0) then + if ( data_in_swapped(DISC_BUILTIN_ENDPOINT_PUBLICATIONS_ANNOUNCER) = '0' or + data_in_swapped(DISC_BUILTIN_ENDPOINT_SUBSCRIPTIONS_DETECTOR) = '0') then + stage_next <= SKIP_PACKET; + end if; + end if; + -- Check for necessary Builtin Endpoints for Writers + if (NUM_WRITERS > 0) then + if ( data_in_swapped(DISC_BUILTIN_ENDPOINT_SUBSCRIPTIONS_ANNOUNCER) = '0' or + data_in_swapped(DISC_BUILTIN_ENDPOINT_PUBLICATIONS_DETECTOR) = '0') then + stage_next <= SKIP_PACKET; + end if; + end if; + end if; + when LATCH_BUILTIN_ENDPOINT_QOS => + if (empty = '0') then + rd_sig <= '1'; + -- Latch QoS + participant_message_best_effort_next <= data_in_swapped(BEST_EFFORT_PARTICIPANT_MESSAGE_DATA_READER); + + -- DEFAULT Next Stage + stage_next <= SKIP_PARAMETER; + end if; + --############################# + when SKIP_PARAMETER => + -- End of Parameter + if (read_cnt > parameter_end) then + -- Begin parsing of next parameter + stage_next <= PROCESS_PL; + else + if (empty = '0') then + rd_sig <= '1'; + if (read_cnt = parameter_end) then + -- Begin parsing of next parameter + stage_next <= PROCESS_PL; + end if; + end if; + end if; + when SKIP_PACKET => + -- TODO: Handle read_cnt > packet_length (Like above) + if (empty = '0') then + rd_sig <= '1'; + -- End of Packet + if (read_cnt = packet_length) then + -- Continue parsing next packet + stage_next <= INIT; + end if; + end if; + when others => + null; + end case; + end process; + + mem_ctrl_prc : process(all) + begin + case (mem_stage) is + + 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') then + read_cnt <= read_cnt + to_unsigned(1, read_cnt'length); + end if; + end if; + end process; + +end architecture; diff --git a/src/rtps_handler.vhd b/src/rtps_handler.vhd index 9495132..8b7a65e 100644 --- a/src/rtps_handler.vhd +++ b/src/rtps_handler.vhd @@ -5,21 +5,22 @@ 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 -- Checksum has to be checked before entity rtps_handler is 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 - 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 + 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 + 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 ); end entity; @@ -60,8 +61,8 @@ architecture arch of rtps_handler is -- 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_ENTPOINT, DATA_HEADER, - MATCH_DST_ENDPOINT, PUSH_PAYLOAD_HEADER, DATA_SKIP_HEADER, PUSH_PAYLOAD, CHECK_SUB_END, SKIP_PACKET, + 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); @@ -108,6 +109,8 @@ architecture arch of rtps_handler is 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'); -- 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 @@ -187,7 +190,7 @@ architecture arch of rtps_handler is return ret; end function; - -- Compares argument 'ref' with every elemant of 'ar', and returns the index of the last match. + -- 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 @@ -318,10 +321,11 @@ begin -- 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_ENTPOINT Read EntityID + -- SRC_ENDPOINT Read EntityID -- DATA_HEADER Parse Submessage DATA Subheader -- 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 @@ -344,6 +348,7 @@ begin domain_id_next <= domain_id; flags_next <= flags; src_entityid_next <= src_entityid; + dest_entityid_next <= dest_entityid; user_endpoint_next <= user_endpoint; builtin_endpoint_next <= builtin_endpoint; return_stage_next <= return_stage; @@ -497,9 +502,7 @@ begin if (empty = '0') then rd_sig <= '1'; -- Sender GUID_Prefix - -- Since we already have the Src Addr and Port, we do not need the - -- Guid_prefix of the source. - --TODO <= data_in; + --TODO <= data_in; if (cnt = GUIDPREFIX_WIDTH/32) then -- Next Stage stage_next <= RTPS_SUB_HEADER; @@ -519,10 +522,10 @@ begin if (empty = '0') then rd_sig <= '1'; - -- TODO: Still valid? -- 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 src_is_reader_next <= '0'; @@ -584,7 +587,7 @@ begin stage_next <= EXTRACT_LOCATOR_UDPv4_1; end if; -- Heartbeat (Writer -> Reader, Available SeqNum) - -- STAGE ORDER: MATCH_DST_ENDPOINT -> SRC_ENTPOINT -> PUSH_PAYLOAD_HEADER -> PUSH_PAYLOAD -> RTPS_SUB_HEADER + -- STAGE ORDER: MATCH_DST_ENDPOINT -> SRC_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 @@ -593,10 +596,10 @@ begin else -- Next Stage stage_next <= MATCH_DST_ENDPOINT; - return_stage_next <= SRC_ENTPOINT; + return_stage_next <= SRC_ENDPOINT; end if; -- AckNack (Reader -> Writer, Request SeqNum) - -- STAGE ORDER: SRC_ENTPOINT -> MATCH_DST_ENDPOINT -> PUSH_PAYLOAD_HEADER -> PUSH_PAYLOAD -> RTPS_SUB_HEADER + -- STAGE ORDER: SRC_ENDPOINT -> 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 @@ -606,10 +609,10 @@ begin -- Mark if destination is for writers src_is_reader_next <= '1'; -- Next Stage - stage_next <= SRC_ENTPOINT; + stage_next <= SRC_ENDPOINT; end if; -- GAP (Writer -> Reader, Invalidate SeqNum) - -- STAGE ORDER: MATCH_DST_ENDPOINT -> SRC_ENTPOINT -> PUSH_PAYLOAD_HEADER -> PUSH_PAYLOAD -> RTPS_SUB_HEADER + -- STAGE ORDER: MATCH_DST_ENDPOINT -> SRC_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 @@ -618,10 +621,10 @@ begin else -- Next Stage stage_next <= MATCH_DST_ENDPOINT; - return_stage_next <= SRC_ENTPOINT; + return_stage_next <= SRC_ENDPOINT; end if; -- DATA (Writer -> Reader, SeqNum+Data) - -- STAGE ORDER: DATA_HEADER -> MATCH_DST_ENDPOINT -> SRC_ENTPOINT -> PUSH_PAYLOAD_HEADER -> DATA_SKIP_HEADER -> PUSH_PAYLOAD -> RTPS_SUB_HEADER + -- STAGE ORDER: DATA_HEADER -> MATCH_DST_ENDPOINT -> SRC_ENDPOINT -> PUSH_PAYLOAD_HEADER -> DATA_SKIP_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 @@ -781,19 +784,20 @@ begin if (empty = '0') then rd_sig <= '1'; -- Second Word of INFO_SRC Submessage - if (cnt = 2) then - -- Check Major Protocol Version - if (aligned_data_in(31 downto 24) /= PROTOCOLVERSION_2_4(15 downto 8)) then - -- Protocol Supported, skip rest of Submessage + case (cnt) is + when 2 => + -- Default Next Stage stage_next <= SKIP_SUB; - else - -- Protocol not supported, skip Packet - stage_next <= SKIP_PACKET; - end if; - else - -- TODO: Check how many addres are synthesized for cnt/cnt_next - cnt_next <= cnt + 1; - end if; + -- 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; + end if; + when others => + null; + end case; + -- TODO: Check how many addres 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; @@ -815,7 +819,7 @@ begin -- Latch Input for alignment purposes align_sig_next <= data_in(23 downto 0); end if; - when SRC_ENTPOINT => + when SRC_ENDPOINT => if (empty = '0') then rd_sig <= '1'; -- Latch src Entity ID @@ -843,7 +847,7 @@ begin 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_ENTPOINT; + return_stage_next <= SRC_ENDPOINT; -- Latch Input for alignment purposes align_sig_next <= data_in(23 downto 0); end if; @@ -895,8 +899,10 @@ begin 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); + 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. @@ -919,7 +925,12 @@ begin output_sig <= src_entityid; wr_sig <= '1'; -- Next Stage - if (is_data = '1') then + 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; @@ -930,6 +941,21 @@ begin 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) @@ -1065,6 +1091,7 @@ begin domain_id <= 0; flags <= (others => '0'); src_entityid <= (others => '0'); + dest_entityid <= (others => '0'); user_endpoint <= (others => '0'); numlocators <= (others => '0'); payload_length <= (others => '0'); @@ -1091,6 +1118,7 @@ begin domain_id <= domain_id_next; flags <= flags_next; src_entityid <= src_entityid_next; + dest_entityid <= dest_entityid_next; user_endpoint <= user_endpoint_next; numlocators <= numlocators_next; payload_length <= payload_length_next; @@ -1107,4 +1135,4 @@ begin end if; end process; -end architecture; \ No newline at end of file +end architecture; diff --git a/src/rtps_package.vhd b/src/rtps_package.vhd index 9050c32..0a2038a 100644 --- a/src/rtps_package.vhd +++ b/src/rtps_package.vhd @@ -46,30 +46,92 @@ package rtps_package is -- *DO NOT MODIFY BEGIN* type ENDPOINT_DOMAIN_MAP_TYPE is array (MAX_ENDPOINTS-1 downto 0) of integer; type ENDPOINT_WITH_KEY_TYPE is array (MAX_ENDPOINTS-1 downto 0) of boolean; + type ENDPOINT_TOPIC_STRING_TYPE is array (MAX_ENDPOINTS-1 downto 0) of string(1 to 256); + type ENDPOINT_TOPIC_TYPE is array (MAX_ENDPOINTS-1 downto 0) of std_logic_vector(0 to (256*8)-1); + subtype QOS_TYPE is array (MAX_ENDPOINTS-1 downto 0) of std_logic_vector(31 downto 0); + subtype QOS_SLV_TYPE is array (MAX_ENDPOINTS-1 downto 0) of std_logic_vector(31 downto 0); + type DURATION_TYPE is array (1 downto 0) of unsigned(31 downto 0); + type ENDPOINT_DURATION_TYPE is array (MAX_ENDPOINTS-1 downto 0) of DURATION_TYPE; + + constant DURATION_INFINITE : DURATION_TYPE := (x"7fffffff", x"ffffffff"); + + -- DURABILITY KIND + constant VOLATILE_DURABILITY_QOS : std_logic_vector(31 downto 0) := std_logic_vector(to_unsigned(0,32)); + constant TRANSIENT_LOCAL_DURABILITY_QOS : std_logic_vector(31 downto 0) := std_logic_vector(to_unsigned(1,32)); + constant TRANSIENT_DURABILITY_QOS : std_logic_vector(31 downto 0) := std_logic_vector(to_unsigned(2,32)); + constant PERSISTENT_DURABILITY_QOS : std_logic_vector(31 downto 0) := std_logic_vector(to_unsigned(3,32)); + -- LIVELINESS KIND + constant AUTOMATIC_LIVELINESS_QOS : std_logic_vector(31 downto 0) := std_logic_vector(to_unsigned(0,32)); + constant MANUAL_BY_PARTICIPANT_LIVELINESS_QOS : std_logic_vector(31 downto 0) := std_logic_vector(to_unsigned(1,32)); + constant MANUAL_BY_TOPIC_LIVELINESS_QOS : std_logic_vector(31 downto 0) := std_logic_vector(to_unsigned(2,32)); + -- RELIABILITY KIND + constant BEST_EFFORT_RELIABILITY_QOS : std_logic_vector(31 downto 0) := std_logic_vector(to_unsigned(1,32)); + constant RELIABLE_RELIABILITY_QOS : std_logic_vector(31 downto 0) := std_logic_vector(to_unsigned(2,32)); + -- OWNERSHIP KIND + constant SHARED_OWNERSHIP_QOS : std_logic_vector(31 downto 0) := std_logic_vector(to_unsigned(0,32)); + constant EXCLUSIVE_OWNERSHIP_QOS : std_logic_vector(31 downto 0) := std_logic_vector(to_unsigned(1,32)); + -- DESTINATION ORDER KIND + constant BY_RECEPTION_TIMESTAMP_DESTINATIONORDER_QOS : std_logic_vector(31 downto 0) := std_logic_vector(to_unsigned(0,32)); + constant BY_SOURCE_TIMESTAMP_DESTINATIONORDER_QOS : std_logic_vector(31 downto 0) := std_logic_vector(to_unsigned(1,32)); + -- PRESENTATION KIND + constant INSTANCE_PRESENTATION_QOS : std_logic_vector(31 downto 0) := std_logic_vector(to_unsigned(0,32)); + constant TOPIC_PRESENTATION_QOS : std_logic_vector(31 downto 0) := std_logic_vector(to_unsigned(1,32)); + constant GROUP_PRESENTATION_QOS : std_logic_vector(31 downto 0) := std_logic_vector(to_unsigned(2,32)); + -- HISTORY KIND + constant KEEP_LAST_HISTORY_QOS : std_logic_vector(31 downto 0) := std_logic_vector(to_unsigned(0,32)); + constant KEEP_ALL_HISTORY_QOS : std_logic_vector(31 downto 0) := std_logic_vector(to_unsigned(1,32)); + -- TYPE CONSISTENCY KIND + constant DISALLOW_TYPE_COERCION : std_logic_vector(15 downto 0) := std_logic_vector(to_unsigned(0,32)); + constant ALLOW_TYPE_COERCION : std_logic_vector(15 downto 0) := std_logic_vector(to_unsigned(1,32)); -- *DO NOT MODIFY END* ----------------------------------------------------------------------------------------------------- --***RTPS ENDPOINTS*** -- Array mapping the RTPS Endpoints to their respective Domain IDs -- The index of this array denotes the Endpoint, and the element value the index of the Domain in the 'USER_DOMAIN_ID'. - constant ENDPOINT_DOMAIN_MAP: ENDPOINT_DOMAIN_MAP_TYPE := (0 => 0); + constant ENDPOINT_DOMAIN_MAP : ENDPOINT_DOMAIN_MAP_TYPE := (0 => 0); -- Array denoting if Endpoints use Keyed Topics - constant ENDPOINT_WITH_KEY : ENDPOINT_WITH_KEY_TYPE := (0 => FALSE); - + constant ENDPOINT_WITH_KEY : ENDPOINT_WITH_KEY_TYPE := (0 => FALSE); + -- Array mapping Topic Strings to Endpoints + -- NOTE: All strings have to be padded to 256 characters + constant ENDPOINT_TOPIC_STRING : ENDPOINT_TOPIC_STRING_TYPE := (0 => "Placeholder" & (12 to 256 => NUL)); + constant ENDPOINT_TOPIC : ENDPOINT_TOPIC_TYPE; -- Deferred to Package Body + constant ENDPOINT_DURABILITY : QOS_TYPE := (0 => VOLATILE_DURABILITY_QOS); + constant ENDPOINT_PRESENTATION : QOS_TYPE := (0 => INSTANCE_PRESENTATION_QOS); + constant ENDPOINT_COHERENT_ACCESS : std_logic_vector(MAX_ENDPOINTS-1 downto 0) := (others => '0'); + constant ENDPOINT_ORDERED_ACCESS : std_logic_vector(MAX_ENDPOINTS-1 downto 0) := (others => '0'); + constant ENDPOINT_DEADLINE : ENDPOINT_DURATION_TYPE := (0 => DURATION_INFINITE); --TODO: Assert + constant ENDPOINT_LIVELINESS : QOS_TYPE := (0 => AUTOMATIC_LIVELINESS_QOS); + constant ENDPOINT_LEASE_DURATION : ENDPOINT_DURATION_TYPE := (0 => DURATION_INFINITE); --TODO: Assert + -- Only relevant for Readers + constant ENDPOINT_TIME_BASED_FILTER : ENDPOINT_DURATION_TYPE := (0 => (to_unsigned(0,32),to_unsigned(0,32))); --TODO: Assert + constant ENDPOINT_RELIABILITY : QOS_TYPE := (0 => RELIABLE_RELIABILITY_QOS); + constant ENDPOINT_MAX_BLOCKING_TIME : ENDPOINT_DURATION_TYPE := (0 => (to_unsigned(0,32),to_unsigned(1,32))); + -- Only relevant for Writers + constant ENDPOINT_LIFESPAN : ENDPOINT_DURATION_TYPE := (0 => DURATION_INFINITE); + constant ENDPOINT_DESTINATION_ORDER : QOS_TYPE := (0 => BY_RECEPTION_TIMESTAMP_DESTINATIONORDER_QOS); + constant ENDPOINT_HISTORY : QOS_TYPE := (0 => KEEP_LAST_HISTORY_QOS); + constant ENDPOINT_DEPTH : QOS_SLV_TYPE := (0 => std_logic_vector(to_unsigned(1,32))); --TODO: Assert + constant ENDPOINT_MAX_SAMPLES : QOS_SLV_TYPE := (0 => std_logic_vector(to_unsigned(0,32))); --TODO: Assert + constant ENDPOINT_MAX_INSTANCES : QOS_SLV_TYPE := (0 => std_logic_vector(to_unsigned(0,32))); --TODO: Assert + constant ENDPOINT_MAX_SAMP_PER_INST : QOS_SLV_TYPE := (0 => std_logic_vector(to_unsigned(0,32))); --TODO: Assert --*****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 GUIDPREFIX_WIDTH : integer := 96; - constant PROTOCOLVERSION_WIDTH : integer := 16; - constant VENDORID_WIDTH : integer := 16; - constant SUBMESSAGE_ID_WIDTH : integer := 8; - constant DOMAIN_ID_WIDTH : integer := 32; - constant UDP_PORT_WIDTH : integer := 16; - constant ENTITYID_WIDTH : integer := 32; - constant PROTOCOL_WIDTH : integer := 32; + constant GUIDPREFIX_WIDTH : integer := 96; + constant PROTOCOLVERSION_WIDTH : integer := 16; + constant VENDORID_WIDTH : integer := 16; + constant SUBMESSAGE_ID_WIDTH : integer := 8; + constant DOMAIN_ID_WIDTH : integer := 32; + constant UDP_PORT_WIDTH : integer := 16; + constant ENTITYID_WIDTH : integer := 32; + constant PROTOCOL_WIDTH : integer := 32; + constant PARAMETER_ID_WIDTH : integer := 16; + constant PAYLOAD_REPRESENTATION_ID : integer := 16; + constant PAYLOAD_REPRESENTATION_OPTIONS : integer := 16; -- 'RTPS' in Ascii code constant PROTOCOL_RTPS : std_logic_vector(PROTOCOL_WIDTH-1 downto 0) := x"52545053"; @@ -90,6 +152,83 @@ package rtps_package is constant SID_HEARTBEAT_FRAG : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"13"; constant SID_DATA : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"15"; constant SID_DATA_FRAG : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"16"; + -- Parameter IDs + constant PID_PAD : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0000"; + constant PID_SENTINEL : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0001"; + constant PID_TOPIC_NAME : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0005"; + constant PID_TYPE_NAME : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0007"; + constant PID_USER_DATA : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"002c"; + constant PID_GROUP_DATA : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"002d"; + constant PID_TOPIC_DATA : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"002e"; + constant PID_DURABILITY : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"001d"; + constant PID_DURABILITY_SERVICE : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"001e"; + constant PID_DEADLINE : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0023"; + constant PID_LATENCY_BUDGET : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0027"; + constant PID_LIVELINESS : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"001b"; + constant PID_RELIABILITY : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"001a"; + constant PID_LIFESPAN : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"002b"; + constant PID_DESTINATION_ORDER : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0025"; + constant PID_HISTORY : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0040"; + constant PID_RESOURCE_LIMITS : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0041"; + constant PID_OWNERSHIP : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"001f"; + constant PID_OWNERSHIP_STRENGTH : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"001e"; + constant PID_PRESENTATION : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0021"; + constant PID_PARTITION : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0029"; + constant PID_TIME_BASED_FILTER : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0004"; + constant PID_TRANSPORT_PRIORITY : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0049"; + constant PID_DOMAIN_ID : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"000f"; + constant PID_DOMAIN_TAG : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"4014"; + constant PID_PROTOCOL_VERSION : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0015"; + constant PID_VENDORID : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0016"; + constant PID_UNICAST_LOCATOR : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"002f"; + constant PID_MULTICAST_LOCATOR : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0030"; + constant PID_DEFAULT_UNICAST_LOCATOR : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0031"; + constant PID_DEFAULT_MULTICAST_LOCATOR : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0048"; + constant PID_METATRAFFIC_UNICAST_LOCATOR : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0032"; + constant PID_METATRAFFIC_MULTICAST_LOCATOR : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0033"; + constant PID_EXPECTS_INLINE_QOS : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0043"; + constant PID_PARTICIPANT_MANUAL_LIVELINESS_COUNT : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0034"; + constant PID_PARTICIPANT_LEASE_DURATION : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0002"; + constant PID_CONTENT_FILTER_PROPERTY : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0035"; + constant PID_PARTICIPANT_GUID : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0050"; + constant PID_GROUP_GUID : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0052"; + 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_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 + constant PID_CONTENT_FILTER_INFO : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0055"; + constant PID_COHERENT_SET : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0056"; + constant PID_DIRECTED_WRITE : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0057"; + constant PID_ORIGINAL_WRITER_INFO : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0061"; + constant PID_GROUP_COHERENT_SET : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0063"; + constant PID_GROUP_SEQ_NUM : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0064"; + constant PID_WRITER_GROUP_INFO : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0065"; + constant PID_SECURE_WRITER_GROUP_INFO : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0066"; + constant PID_KEY_HASH : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0070"; + constant PID_STATUS_INFO : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"0071"; + -- XTYPES 1.3 + constant PID_EXTENDED : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"7F01"; + constant PID_LIST_END : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"7F02"; + constant PID_IGNORE : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) := x"3F03"; + + + -- PAYLOAD REPRESENTATION IDENTIFIERS + constant CDR_BE : std_logic_vector(PAYLOAD_REPRESENTATION_ID-1 downto 0) := x"0000"; + constant CDR_LE : std_logic_vector(PAYLOAD_REPRESENTATION_ID-1 downto 0) := x"0001"; + constant PL_CDR_BE : std_logic_vector(PAYLOAD_REPRESENTATION_ID-1 downto 0) := x"0002"; + constant PL_CDR_LE : std_logic_vector(PAYLOAD_REPRESENTATION_ID-1 downto 0) := x"0003"; + constant XML : std_logic_vector(PAYLOAD_REPRESENTATION_ID-1 downto 0) := x"0004"; + constant CDR2_BE : std_logic_vector(PAYLOAD_REPRESENTATION_ID-1 downto 0) := x"0010"; + constant CDR2_LE : std_logic_vector(PAYLOAD_REPRESENTATION_ID-1 downto 0) := x"0011"; + constant PL_CDR2_BE : std_logic_vector(PAYLOAD_REPRESENTATION_ID-1 downto 0) := x"0012"; + constant PL_CDR2_LE : std_logic_vector(PAYLOAD_REPRESENTATION_ID-1 downto 0) := x"0013"; + constant D_CDR_BE : std_logic_vector(PAYLOAD_REPRESENTATION_ID-1 downto 0) := x"0014"; + constant D_CDR_LE : std_logic_vector(PAYLOAD_REPRESENTATION_ID-1 downto 0) := x"0015"; + + @@ -102,7 +241,7 @@ package rtps_package is -- Since this implementation runs on the same network stack and the RTPS Endpoints (Readers & Writers) -- can be differentiated based on their Entity ID, it makes no sense to have multiple IP Addresses -- (and hence multiple Participants). - -- We generate just single participant for every Domain, and later match the Endpoints to their respective + -- We generate just a single participant for every Domain, and later match the Endpoints to their respective -- Domain (and thus also to their respective RTPS Participant). type IPv4_PORT_TYPE is array (NUM_DOMAIN-1 downto 0) of std_logic_vector(UDP_PORT_WIDTH-1 downto 0); @@ -112,6 +251,8 @@ package rtps_package is constant USER_IPv4_UNICAST_PORT : IPv4_PORT_TYPE; -- Deferred to Package Body type GUIDPREFIX_TYPE is array (NUM_DOMAIN-1 downto 0) of std_logic_vector(GUIDPREFIX_WIDTH-1 downto 0); + type GUIDPREFIX_ARRAY_TYPE is array (GUIDPREFIX_WIDTH/32-1 downto 0) of std_logic_vector(31 downto 0); + type GUID_ARRAY_TYPE is array ((GUIDPREFIX_WIDTH+ENTITYID_WIDTH)/32-1 downto 0) of std_logic_vector(31 downto 0); constant GUIDPREFIX : GUIDPREFIX_TYPE; -- Deferred to Package Body constant GUIDPREFIX_UNKNOWN : std_logic_vector(GUIDPREFIX_WIDTH-1 downto 0) := (others => '0'); @@ -152,6 +293,21 @@ package rtps_package is constant LOCATOR_KIND_UDPv4 : std_logic_vector := std_logic_vector(to_signed(1,LOCATOR_KIND_WIDTH)); constant LOCATOR_KIND_UDPv6 : std_logic_vector := std_logic_vector(to_signed(2,LOCATOR_KIND_WIDTH)); + -- BUILTIN ENDPOINT SET POSITIONS + constant DISC_BUILTIN_ENDPOINT_PARTICIPANT_ANNOUNCER : integer := 0; + constant DISC_BUILTIN_ENDPOINT_PARTICIPANT_DETECTOR : integer := 1; + constant DISC_BUILTIN_ENDPOINT_PUBLICATIONS_ANNOUNCER : integer := 2; + constant DISC_BUILTIN_ENDPOINT_PUBLICATIONS_DETECTOR : integer := 3; + constant DISC_BUILTIN_ENDPOINT_SUBSCRIPTIONS_ANNOUNCER : integer := 4; + constant DISC_BUILTIN_ENDPOINT_SUBSCRIPTIONS_DETECTOR : integer := 5; + constant BUILTIN_ENDPOINT_PARTICIPANT_MESSAGE_DATA_WRITER : integer := 10; + constant BUILTIN_ENDPOINT_PARTICIPANT_MESSAGE_DATA_READER : integer := 11; + constant DISC_BUILTIN_ENDPOINT_TOPICS_ANNOUNCER : integer := 28; + constant DISC_BUILTIN_ENDPOINT_TOPICS_DETECTOR : integer := 29; + + -- BUILTIN ENDPOINT QOS BITMASK + constant BEST_EFFORT_PARTICIPANT_MESSAGE_DATA_READER : integer := 0; + --*****CUSTOM***** --**************** @@ -296,4 +452,18 @@ package body rtps_package is constant ENTITYID : ENTITYID_TYPE := gen_entyid; + function convert_string (string_array : ENDPOINT_TOPIC_STRING_TYPE) return ENDPOINT_TOPIC_TYPE is + variable ret : ENDPOINT_TOPIC_TYPE + begin + ret := (others => (others => '0')); + for i in 0 to ret'length-1 loop + for j in 1 to ret'length(1) loop + ret(i)((256-j)*8 to ((257-j)*8)-1) := std_logic_vector(to_unsigned(character'POS(string_array(i)(j)), 8)); + end loop; + end loop; + return ret; + end function; + + constant ENDPOINT_TOPIC : ENDPOINT_TOPIC_TYPE := convert_string(ENDPOINT_TOPIC_STRING); + end package body;