diff --git a/src/rtps_builtin_endpoint.vhd b/src/rtps_builtin_endpoint.vhd index ffd7c0f..ebb154e 100644 --- a/src/rtps_builtin_endpoint.vhd +++ b/src/rtps_builtin_endpoint.vhd @@ -81,6 +81,7 @@ architecture arch of rtps_builtin_endpoint is constant MES_DATA_FLAG : natural := 29; constant PUB_SEQUENCE_NR : DOUBLE_WORD_ARRAY := convert_to_double_word(to_unsigned(NUM_WRITERS, 64)); constant SUB_SEQUENCE_NR : DOUBLE_WORD_ARRAY := convert_to_double_word(to_unsigned(NUM_READERS, 64)); + constant SEQUENCE_NR_START : DOUBLE_WORD_ARRAY := convert_to_double_word(to_unsigned(1, 64)); constant ZERO_PARTICIPANT_DATA : PARTICIPANT_DATA_TYPE := ( meta_addr => (others => '0'), def_addr => (others => '0'), @@ -100,7 +101,8 @@ architecture arch of rtps_builtin_endpoint is --*****SIGNAL DECLARATION***** -- FSM state - signal stage, stage_next : STAGE_TYPE := SRC_ADDR_HEADER; + signal stage, stage_next : STAGE_TYPE := IDLE; + signal return_stage, return_stage_next : STAGE_TYPE := IDLE; 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'; @@ -183,6 +185,9 @@ architecture arch of rtps_builtin_endpoint is signal live_gap_start, live_gap_start_next : DOUBLE_WORD_ARRAY := (others => (others => '0')); signal live_gap_end, live_gap_end_next : DOUBLE_WORD_ARRAY := (others => (others => '0')); signal extra_flags, extra_flags_next : std_logic_vector(31 downto 0) := (others => '0'); + signal announcement_time, announcement_time_next : DOUBLE_WORD_ARRAY := (others => (others => '0')); + signal heartbeat_time, heartbeat_time_next : DOUBLE_WORD_ARRAY := (others => (others => '0')); + signal is_live_assert, is_live_assert_next : std_logic := '0'; @@ -357,6 +362,7 @@ begin begin --DEFAULT stage_next <= stage; + return_stage_next <= return_stage; packet_length_next <= packet_length; rd_sig <= '0'; reset_read_cnt <= '0'; @@ -400,6 +406,9 @@ begin man_live_seq_nr_next <= man_live_seq_nr; live_gap_start_next <= live_gap_start; live_gap_end_next <= live_gap_end; + announcement_time_next <= announcement_time; + heartbeat_time_next <= heartbeat_time; + is_live_assert_next <= is_live_assert; -- Last Word Latch Setter if (last_word_in = '1') then @@ -410,21 +419,44 @@ begin end if; -- TODO: Reset Latches + -- TODO: Increment counters before send + -- TODO: Cast unsigned to std_logic_vector + -- TODO: Always check addr_res on completed memory operation case(stage) is -- Initial/Idle State when IDLE => - -- No Packets to process - if (empty = '1') then - mem_opcode <= FIND_STALE_PARTICIPANT; - start_mem_op <= '1'; - stale_check_next <= '1'; - stage_next <= STALE_CHECK; - elsif (stale_check = '0') then - mem_opcode <= FIND_STALE_PARTICIPANT; - start_mem_op <= '1'; - stale_check_next <= '1'; - stage_next <= STALE_CHECK; + -- Participant Announcement Trigger + if (time > announcement_time) then + announcement_time_next <= time + TODO; + stage_next <= SEND_PARTICIPANT_ANNOUNCEMENT_1; + -- Heartbeat Trigger + elsif (time > heartbeat_time) then + if (mem_op_done = '1') then + if (endpoint_alive = '1') then + reset_endpoint_alive <= '1'; + man_live_seq_nr_next <= auto_live_seq_nr + 1; + live_gap_start_next <= auto_live_seq_nr + 2; + auto_live_seq_nr_next <= auto_live_seq_nr + 2; + live_gap_end_next <= auto_live_seq_nr + 1; + else + auto_live_seq_nr_next <= auto_live_seq_nr + 1; + live_gap_end_next <= live_gap_end + 1; + end if; + heartbeat_time_next <= time + TODO; + mem_opcode <= FIND_FIRST_PATICIPANT; + start_mem_op <= '1'; + stage_next <= FIND_PARTICIPANT_DEST; + is_live_assert_next <= '1'; + end if; + -- No Packet to process + elsif (stale_check = '0' or empty = '1') then + if (mem_op_done = '1') then + mem_opcode <= FIND_STALE_PARTICIPANT; + start_mem_op <= '1'; + stale_check_next <= '1'; + stage_next <= STALE_CHECK; + end if; else -- Process Packet stale_check_next <= '0'; @@ -1768,7 +1800,8 @@ begin update_participant_flags_next <= (HEARTBEAT_RES_TIME_FLAG_FLAG => '1', others => '0'); start_mem_op <= '1'; -- Send Acknack - stage_next <= SEND_ACKNACK; + stage_next <= SEND_HEADER; + return_stage_next <= SEND_ACKNACK; cnt_next <= 0; end if; -- Acknack Response Time @@ -1802,7 +1835,7 @@ begin update_participant_flags_next <= (EXTRA_FLAGS_FLAG => '1', ACKNACK_RES_TIME_FLAG => '1', others => '0'); start_mem_op <= '1'; -- Send Requested Data - stage_next <= SEND_PUB_DATA; + stage_next <= SEND_DATA; cnt_next <= 0; end if; end if; @@ -1885,98 +1918,171 @@ begin cnt_next <= cnt + 1; case (cnt) is - -- OUTPUT HEADER - -- Src IPv4 Address - when 0 => - output_sig <= DEFAULT_IPv4_MULTICAST_ADDRESS; - -- Dest IPv4 Address - when 1 => - output_sig <= mem_participant_data.meta_addr; - -- Src and Dest UDPv4 Ports - when 2 => - output_sig <= META_IPv4_UNICAST_PORT & mem_participant_data.meta_port; - -- RTPS MESSAGE HEADER - when 3 => - output_sig <= PROTOCOL_RTPS; - when 4 => - output_sig <= PROTOCOLVERSION_2_4 & VENDORID; - when 5 => - output_sig <= GUIDPREFIX(0); - when 6 => - output_sig <= GUIDPREFIX(1); - when 7 => - output_sig <= GUIDPREFIX(2); -- ACKNACK RTPS SUBMESSAGE (Publication) -- RTPS Submessage Header - when 8 => + when 0 => output_sig <= SID_ACKNACK & "00000010" & std_logic_vector(to_unsigned(24, 16)); -- Reader Entity ID - when 9 => + when 1 => output_sig <= ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR; -- Writer Entity ID - when 10 => + when 2 => output_sig <= ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER; -- Sequence Number Set (Bitmap Base 1/2) - when 11 => + when 3 => output_sig <= mem_participant_data.pub_seq_nr(0); -- Sequence Number Set (Bitmap Base 2/2) - when 12 => + when 4 => output_sig <= mem_participant_data.pub_seq_nr(1); -- Sequence Number Set (NumBits) - when 13 => + when 5 => output_sig <= (others => '0'); -- Count - when 14 => + when 6 => output_sig <= count; -- ACKNACK RTPS SUBMESSAGE (Subscription) -- RTPS Submessage Header - when 15 => + when 7 => output_sig <= SID_ACKNACK & "00000010" & std_logic_vector(to_unsigned(24, 16)); -- Reader Entity ID - when 16 => + when 8 => output_sig <= ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR; -- Writer Entity ID - when 17 => + when 9 => output_sig <= ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER; -- Sequence Number Set (Bitmap Base 1/2) - when 18 => + when 10 => output_sig <= mem_participant_data.sub_seq_nr(0); -- Sequence Number Set (Bitmap Base 2/2) - when 19 => + when 11 => output_sig <= mem_participant_data.sub_seq_nr(1); -- Sequence Number Set (NumBits) - when 20 => + when 12 => output_sig <= (others => '0'); -- Count - when 21 => + when 13 => output_sig <= count; -- ACKNACK RTPS SUBMESSAGE (Message) -- RTPS Submessage Header - when 22 => + when 14 => output_sig <= SID_ACKNACK & "00000010" & std_logic_vector(to_unsigned(24, 16)); -- Reader Entity ID - when 23 => + when 15 => output_sig <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER; -- Writer Entity ID - when 24 => + when 16 => output_sig <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER; -- Sequence Number Set (Bitmap Base 1/2) - when 25 => + when 17 => output_sig <= mem_participant_data.mes_seq_nr(0); -- Sequence Number Set (Bitmap Base 2/2) - when 26 => + when 18 => output_sig <= mem_participant_data.mes_seq_nr(1); -- Sequence Number Set (NumBits) - when 27 => + when 19 => output_sig <= (others => '0'); -- Count - when 28 => + when 20 => output_sig <= count; -- Signal Last Word7 first_seq_nr_next(1) <= data_in; stage_next <= PROCESS_ACKNACK_SEQUENCE_NUMBERS end case; end if; + when SEND_HEARTBEAT => + if (rtps_full = '0') then + wr_sig <= '1'; + --Increment Counter + cnt_next <= cnt + 1; + + case (cnt) is + -- HEARTBEAT RTPS SUBMESSAGE (Publication) + -- RTPS Submessage Header + when 0 => + output_sig <= SID_HEARTBEAT & "00000010" & std_logic_vector(to_unsigned(28, 16)); + -- Reader Entity ID + when 1 => + output_sig <= ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR; + -- Writer Entity ID + when 2 => + output_sig <= ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER; + -- Sequence Number 1/2 + when 3 => + output_sig <= SEQUENCE_NR_START(0); + -- Sequence Number 2/2 + when 4 => + output_sig <= SEQUENCE_NR_START(1); + -- Sequence Number 1/2 + when 5 => + output_sig <= PUB_SEQUENCE_NR(0); + -- Sequence Number 1/2 + when 6 => + output_sig <= PUB_SEQUENCE_NR(1); + -- Count + when 7 => + output_sig <= count; + -- HEARTBEAT RTPS SUBMESSAGE (Subscription) + -- RTPS Submessage Header + when 8 => + output_sig <= SID_HEARTBEAT & "00000010" & std_logic_vector(to_unsigned(28, 16)); + -- Reader Entity ID + when 9 => + output_sig <= ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR; + -- Writer Entity ID + when 10 => + output_sig <= ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER; + -- Sequence Number 1/2 + when 11 => + output_sig <= SEQUENCE_NR_START(0); + -- Sequence Number 2/2 + when 12 => + output_sig <= SEQUENCE_NR_START(1); + -- Sequence Number 1/2 + when 13 => + output_sig <= SUB_SEQUENCE_NR(0); + -- Sequence Number 1/2 + when 14 => + output_sig <= SUB_SEQUENCE_NR(1); + -- Count + when 15 => + output_sig <= count; + -- HEARTBEAT RTPS SUBMESSAGE (Message) + -- RTPS Submessage Header + when 16 => + output_sig <= SID_HEARTBEAT & "00000010" & std_logic_vector(to_unsigned(28, 16)); + -- Reader Entity ID + when 17 => + output_sig <= ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR; + -- Writer Entity ID + when 18 => + output_sig <= ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER; + -- Sequence Number 1/2 + when 19 => + output_sig <= man_live_seq_nr(0); + -- Sequence Number 2/2 + when 20 => + output_sig <= man_live_seq_nr(1); + -- Sequence Number 1/2 + when 21 => + output_sig <= auto_live_seq_nr(0); + -- Sequence Number 1/2 + when 22 => + output_sig <= auto_live_seq_nr(1); + -- Count + when 23 => + output_sig <= count; + -- If manual and automatic sequence numbers are not consecutive, we have only asserted the automatic liveliness + if (live_gap_start /= auto_live_seq_nr) then + stage_next <= SEND_MES_AUTO_LIVE; + cnt_next <= 0; + -- Else we need to send both (manual and automatic) + else + stage_next <= SEND_MES_MAN_LIVE; + cnt_next <= 0; + end if; + if () + end case; + end if; when PROCESS_ACKNACK_SEQUENCE_NUMBERS => -- Wait for Memory Operation to finish if (mem_done = '1') then @@ -2086,6 +2192,70 @@ begin -- Done stage_next <= SKIP_PACKET; end if; + when FIND_PARTICIPANT_DEST => + if (mem_op_done = '1') then + -- No Participant Found + if (addr_res = BUILTIN_BUFFER_SIZE) then + stage_next <= IDLE; + else + cnt_next <= 0; + stage_next <= SEND_HEADER; + return_stage_next <= SEND_HEARTBEAT; + end if + end if; + when SEND_HEADER => + if (rtps_full = '0') then + wr_sig <= '1'; + --Increment Counter + cnt_next <= cnt + 1; + + case (cnt) is + -- OUTPUT HEADER + -- Src IPv4 Address + when 0 => + output_sig <= DEFAULT_IPv4_MULTICAST_ADDRESS; + -- Dest IPv4 Address + when 1 => + if (return_stage = SEND_PARTICIPANT_ANNOUNCEMENT) then + output_sig <= DEFAULT_IPv4_MULTICAST_ADDRESS; + else + output_sig <= mem_participant_data.meta_addr; + end if; + -- Src and Dest UDPv4 Ports + when 2 => + if (return_stage = SEND_PARTICIPANT_ANNOUNCEMENT) then + output_sig <= META_IPv4_UNICAST_PORT & META_IPv4_MULTICAST_PORT; + else + output_sig <= META_IPv4_UNICAST_PORT & mem_participant_data.meta_port; + end if; + -- RTPS MESSAGE HEADER + when 3 => + output_sig <= PROTOCOL_RTPS; + when 4 => + output_sig <= PROTOCOLVERSION_2_4 & VENDORID; + when 5 => + output_sig <= GUIDPREFIX(0); + when 6 => + output_sig <= GUIDPREFIX(1); + when 7 => + output_sig <= GUIDPREFIX(2); + cnt_next <= 0; + stage_next <= return_stage; + end case; + end if; + when SEND_PARTICIPANT_ANNOUNCEMENT => + if (rtps_full = '0') then + wr_sig <= '1'; + --Increment Counter + cnt_next <= cnt + 1; + -- Send Data + output_sig <= PARTICIPANT_DATA.data(cnt); + -- Exit Condition + if (cnt = (PARTICIPANT_DATA.length-1)) then + last_word_out <= '1'; + stage_next <= IDLE; + end if; + end if; when SEND_PUB_DATA => -- If Publisher Data not scheduled for response or no Writers available, skip if (mem_participant_data.extra_flags(PUB_DATA_FLAG) = '0' or NUM_WRITERS = 0) then @@ -2099,14 +2269,19 @@ begin -- Exit Condition if (cnt = (WRITER_ENDPOINT_DATA.length-1)) then last_word_out <= '1'; - cnt_next <= 0; - stage_next <= SEND_SUB_DATA; + if (mem_participant_data.extra_flags(SUB_DATA_FLAG) = '1' or mem_participant_data.extra_flags(MES_DATA_FLAG) = '1') then + stage_next <= SEND_HEADER; + return_stage_next <= SEND_SUB_DATA; + cnt_next <= 0; + else + stage_next <= IDLE; + end if; end if; end if; when SEND_SUB_DATA => -- If Subscriber Data not scheduled for response or no Readers available, skip if (mem_participant_data.extra_flags(PUB_DATA_FLAG) = '0' or NUM_READERS = 0) then - stage_next <= SEND_MES_DATA; + stage_next <= SEND_MES_MAN_LIVE; elsif (rtps_full = '0') then wr_sig <= '1'; --Increment Counter @@ -2116,45 +2291,15 @@ begin -- Exit Condition if (cnt = (READER_ENDPOINT_DATA.length-1)) then last_word_out <= '1'; - cnt_next <= 0; - stage_next <= SEND_MES_DATA; + if (mem_participant_data.extra_flags(MES_DATA_FLAG) = '1') then + stage_next <= SEND_HEADER; + return_stage_next <= SEND_MES_MAN_LIVE; + cnt_next <= 0; + else + stage_next <= IDLE; + end if; end if; end if; - when SEND_MES_DATA => - -- If Message Data not scheduled for response, skip - if (mem_participant_data.extra_flags(MES_DATA_FLAG) = '0') then - stage_next <= IDLE; - elsif (rtps_full = '0') then - wr_sig <= '1'; - --Increment Counter - cnt_next <= cnt + 1; - - case (cnt) is - -- OUTPUT HEADER - -- Src IPv4 Address - when 0 => - output_sig <= DEFAULT_IPv4_MULTICAST_ADDRESS; - -- Dest IPv4 Address - when 1 => - output_sig <= mem_participant_data.meta_addr; - -- Src and Dest UDPv4 Ports - when 2 => - output_sig <= META_IPv4_UNICAST_PORT & mem_participant_data.meta_port; - -- RTPS MESSAGE HEADER - when 3 => - output_sig <= PROTOCOL_RTPS; - when 4 => - output_sig <= PROTOCOLVERSION_2_4 & VENDORID; - when 5 => - output_sig <= GUIDPREFIX(0); - when 6 => - output_sig <= GUIDPREFIX(1); - when 7 => - output_sig <= GUIDPREFIX(2); - cnt_next <= 0; - stage_next <= SEND_MAN_LIVE; - end case; - end if; when SEND_MES_MAN_LIVE => if (rtps_full = '0') then wr_sig <= '1'; @@ -2293,7 +2438,15 @@ begin when 11 => output_sig <= (others => '0'); last_word_out <= '1'; - stage_next <= IDLE; + -- If we are in the middle of a liveliness assertion, find the next participant destination + if (is_live_assert = '1') then + stage_next <= FIND_PARTICIPANT_DEST; + mem_opcode <= FIND_NEXT_PARTICIPANT; + start_mem_op <= '1'; + else + -- DONE + stage_next <= IDLE; + end if; end case; end if; when LIVELINESS_UPDATE => @@ -2436,6 +2589,17 @@ begin mem_addr_base_next <= tmp; mem_addr_next <= tmp; mem_cnt_next <= 0; + when FIND_FIRST_PATICIPANT => + mem_stage_next <= FIND_NEXT_PARTICIPANT; + tmp := (others => '0'); + mem_addr_base_next <= tmp; + mem_addr_next <= tmp; + mem_cnt_next <= 0; + when FIND_NEXT_PARTICIPANT => + mem_stage_next <= FIND_NEXT_PARTICIPANT; + mem_addr_base_next <= addr_res; + mem_addr_next <= addr_res; + mem_cnt_next <= 0; when UPDATE_PARTICIPANT => mem_stage_next <= UPDATE_PARTICIPANT; if (update_participant_flags(PARTICIPANT_DATA_FLAG) = '1') then @@ -2932,6 +3096,54 @@ begin mem_cnt_next <= 0; endpoint_mask_array_next <= convert_to_bitmask_array(endpoint_mask); end case; + when FIND_NEXT_PARTICIPANT => + mem_rd <= '1'; + -- Increment counter + mem_cnt_next <= mem_cnt + 1; + -- Default Address Increment + mem_addr_next <= mem_addr + 1; + -- Next Participant Frame Address + tmp := mem_addr_base + PARTICIPANT_FRAME_SIZE; + tmp2 := mem_addr_base + 3; + + case (mem_cnt) is + when 0 => + -- Preload + -- Reached MAX Addr, No Match Found + if (mem_addr_base = max_participant_addr) then + addr_res_next <= to_unsigned(BUILTIN_BUFFER_SIZE, addr_res'length); --No match + mem_stage_next <= IDLE; + end if; + when 1 => + -- If slot occupied, get participant data + if (mem_read_data /= GUIDPREFIX_UNKNOWN_ARRAY(0)) then + mem_addr_next <= tmp2; + mem_cnt_next <= 0; + mem_stage_next <= GET_PARTICIPANT_DATA; + addr_res_next <= mem_addr_base; + end if; + when 2 => + -- If slot occupied, jump to stale check + if (mem_read_data /= GUIDPREFIX_UNKNOWN_ARRAY(1)) then + mem_addr_next <= tmp2; + mem_cnt_next <= 0; + mem_stage_next <= GET_PARTICIPANT_DATA; + addr_res_next <= mem_addr_base; + end if; + when 3 => + -- If slot occupied, jump to stale check + if (mem_read_data /= GUIDPREFIX_UNKNOWN_ARRAY(2)) then + mem_addr_next <= tmp2; + mem_cnt_next <= 0; + mem_stage_next <= GET_PARTICIPANT_DATA; + addr_res_next <= mem_addr_base; + -- Slot empty, check next slot + else + mem_addr_next <= tmp; + mem_addr_base_next <= tmp; + mem_cnt_next <= 0; + end if; + end case; when FIND_STALE_PARTICIPANT => mem_rd <= '1'; -- Increment counter diff --git a/src/rtps_package.vhd b/src/rtps_package.vhd index e2982c8..2824a44 100644 --- a/src/rtps_package.vhd +++ b/src/rtps_package.vhd @@ -468,8 +468,8 @@ package body rtps_package is end function; function gen_reader_endpoint_data return OUTPUT_DATA_TYPE is - -- Limit DATA to MAX UDPv4 Payload Size (65,507 Bytes) - variable ret : OUTPUT_DATA_TYPE(data(0 to 16376)) := (data => (others => (others => '0')), length => 0); + -- Limit DATA to MAX UDPv4 Payload Size - RTPS Header (65487 Bytes) + variable ret : OUTPUT_DATA_TYPE(data(0 to 16371)) := (data => (others => (others => '0')), length => 0); variable ind : natural := 0; variable len : natural := 0; variable tmp : natural := 0; @@ -483,13 +483,7 @@ package body rtps_package is len := 0; ind := 0; ind2 := 0; - -- RTPS HEADER - ret.data(0) := PROTOCOL_RTPS; - ret.data(1) := PROTOCOLVERSION_2_4 & VENDORID; - ret.data(2) := GUIDPREFIX(0); - ret.data(3) := GUIDPREFIX(1); - ret.data(4) := GUIDPREFIX(2); - ind := 5; + -- RTPS Submessages -- One DATA Submessage for each Reader for i in 0 to NUM_READERS-1 loop @@ -526,12 +520,10 @@ package body rtps_package is len := len + 1; ret.data(ind+len) := ENTITYID(i); -- EXPECTS INLINE QOS (Only relevant for Reader endpoints) - if (i < NUM_READERS) then - len := len + 1; - ret.data(ind+len) := PID_EXPECTS_INLINE_QOS & std_logic_vector(to_unsigned(4, 16)); - len := len + 1; - ret.data(ind+len) := (24 => '1', others => '0'); - end if; + len := len + 1; + ret.data(ind+len) := PID_EXPECTS_INLINE_QOS & std_logic_vector(to_unsigned(4, 16)); + len := len + 1; + ret.data(ind+len) := (24 => '1', others => '0'); -- TOPIC NAME tmp := string_len(ENDPOINT_TOPIC(i)); len := len + 1; @@ -624,8 +616,8 @@ package body rtps_package is constant READER_ENDPOINT_DATA : OUTPUT_DATA_TYPE := gen_reader_endpoint_data; function gen_writer_endpoint_data return OUTPUT_DATA_TYPE is - -- Limit DATA to MAX UDPv4 Payload Size (65,507 Bytes) - variable ret : OUTPUT_DATA_TYPE(data(0 to 16376)) := (data => (others => (others => '0')), length => 0); + -- Limit DATA to MAX UDPv4 Payload Size - RTPS Header (65487 Bytes) + variable ret : OUTPUT_DATA_TYPE(data(0 to 16371)) := (data => (others => (others => '0')), length => 0); variable ind : natural := 0; variable len : natural := 0; variable tmp : natural := 0; @@ -639,13 +631,7 @@ package body rtps_package is len := 0; ind := 0; ind2 := 0; - -- RTPS HEADER - ret.data(0) := PROTOCOL_RTPS; - ret.data(1) := PROTOCOLVERSION_2_4 & VENDORID; - ret.data(2) := GUIDPREFIX(0); - ret.data(3) := GUIDPREFIX(1); - ret.data(4) := GUIDPREFIX(2); - ind := 5; + -- RTPS Submessages -- One DATA Submessage for each Writer Endpoint for i in NUM_READERS to MAX_ENDPOINTS-1 loop @@ -681,13 +667,6 @@ package body rtps_package is ret.data(ind+len) := GUIDPREFIX(2); len := len + 1; ret.data(ind+len) := ENTITYID(i); - -- EXPECTS INLINE QOS (Only relevant for Reader endpoints) - if (i < NUM_READERS) then - len := len + 1; - ret.data(ind+len) := PID_EXPECTS_INLINE_QOS & std_logic_vector(to_unsigned(4, 16)); - len := len + 1; - ret.data(ind+len) := (24 => '1', others => '0'); - end if; -- TOPIC NAME tmp := string_len(ENDPOINT_TOPIC(i)); len := len + 1; @@ -789,8 +768,8 @@ package body rtps_package is constant ENDPOINT_DATA : OUTPUT_DATA_TYPE := gen_writer_endpoint_data; function gen_participant_data return OUTPUT_DATA_TYPE is - -- Limit DATA to MAX UDPv4 Payload Size (65,507 Bytes) - variable ret : OUTPUT_DATA_TYPE(data(0 to 16376)) := (data => (others => (others => '0')), length => 0); + -- Limit DATA to MAX UDPv4 Payload Size - RTPS Header (65487 Bytes) + variable ret : OUTPUT_DATA_TYPE(data(0 to 16371)) := (data => (others => (others => '0')), length => 0); variable ind : natural := 0; variable len : natural := 0; variable tmp : natural := 0; @@ -800,13 +779,7 @@ package body rtps_package is len := 0; ind := 0; ind2 := 0; - -- RTPS HEADER - ret.data(0) := PROTOCOL_RTPS; - ret.data(1) := PROTOCOLVERSION_2_4 & VENDORID; - ret.data(2) := GUIDPREFIX(0); - ret.data(3) := GUIDPREFIX(1); - ret.data(4) := GUIDPREFIX(2); - ind := 5; + -- RTPS DATA SUBMESSAGE -- RTPS Submessage Header ret.data(ind) := SID_DATA & "00000100" & x"0000"; @@ -919,7 +892,7 @@ package body rtps_package is len := len + 1; ret.data(ind+len) := LOCATOR_KIND_UDPv4; len := len + 1; - ret.data(ind+len) := (16 downto 0 => META_IPv4_MULTICAST_PORT, others => '0'); + ret.data(ind+len) := (16 downto 0 => USER_IPv4_MULTICAST_PORT, others => '0'); len := len + 1; ret.data(ind+len) := (others => '0'); len := len + 1;