From 69da90be20aaa75e68eac6b81ba8616dabab6519 Mon Sep 17 00:00:00 2001 From: Greek Date: Wed, 21 Oct 2020 16:57:40 +0200 Subject: [PATCH] * Finished ACKNACK and HEARTBEAT handling --- src/rtps_builtin_endpoint.vhd | 509 +++++++++++++++++++++++++++------- src/rtps_package.vhd | 15 + 2 files changed, 429 insertions(+), 95 deletions(-) diff --git a/src/rtps_builtin_endpoint.vhd b/src/rtps_builtin_endpoint.vhd index b351dff..7210f6a 100644 --- a/src/rtps_builtin_endpoint.vhd +++ b/src/rtps_builtin_endpoint.vhd @@ -72,9 +72,13 @@ architecture arch of rtps_builtin_endpoint is constant BUILTIN_BUFFER_ADDR_WIDTH : natural := log2c(BUILTIN_BUFFER_SIZE); constant PARTICIPANT_DATA_FLAG : natural := 0; constant LEASE_DEADLINE_FLAG : natural := 1; - constant HEARTBEAT_RES_TIME_FLAG : natural := 2; + constant EXTRA_FLAGS_FLAG : natural := 2; constant ACKNACK_RES_TIME_FLAG : natural := 3; - constant EDP_SEQ_NR_FLAG : natural := 4; + constant HEARTBEAT_RES_TIME_FLAG : natural := 4; + constant EDP_SEQ_NR_FLAG : natural := 5; + constant PUB_DATA_FLAG : natural := 31; + constant SUB_DATA_FLAG : natural := 30; + 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 ZERO_PARTICIPANT_DATA : PARTICIPANT_DATA_TYPE := ( @@ -166,15 +170,19 @@ architecture arch of rtps_builtin_endpoint is signal stale_check, stale_check_next : std_logic := '0'; signal mem_guidprefix, mem_guidprefix_next : GUIDPREFIX_ARRAY_TYPE : (others => (others => '0')); signal last_word_in_latch, last_word_in_latch_next : std_logic := '0'; - signal update_participant_flags, update_participant_flags_next : std_logic_vector(3 downto 0) := (others => '0'); + signal update_participant_flags, update_participant_flags_next : std_logic_vector(5 downto 0) := (others => '0'); signal mem_participant_data, mem_participant_data_next : PARTICIPANT_DATA_TYPE := ZERO_PARTICIPANT_DATA; signal is_heartbeat_res, is_heartbeat_res_next : std_logic := '0'; signal seq_prc_done, seq_prc_done_next : std_logic := '0'; signal count, count_next : unsigned(31 downto 0) := (others => '0'); signal endpoint_alive : std_logic := '0'; signal reset_endpoint_alive : std_logic := '0'; + -- TODO: Make sure sequences are initialized to 1 signal auto_live_seq_nr, auto_live_seq_nr_next : DOUBLE_WORD_ARRAY := (others => (others => '0')); signal man_live_seq_nr, man_live_seq_nr_next : DOUBLE_WORD_ARRAY := (others => (others => '0')); + 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'); @@ -374,7 +382,7 @@ begin mem_opcode <= NOP; start_mem_op <= '0'; is_orphan_search_next <= is_orphan_search; - participant_message_best_effort_next <= participant_message_best_effort; + extra_flags_next <= extra_flags; output_sig <= (others => '0'); wr_sig <= '0'; stale_check_next <= stale_check; @@ -386,6 +394,8 @@ begin reset_endpoint_alive <= '0'; auto_live_seq_nr_next <= auto_live_seq_nr; man_live_seq_nr_next <= man_live_seq_nr; + live_gap_start_next <= live_gap_start; + live_gap_end_next <= live_gap_end; -- Last Word Latch Setter if (last_word_in = '1') then @@ -501,7 +511,9 @@ begin -- Only ACKNACKs are relevant if (NUM_WRITERS /= 0 and opcode = SID_ACKNACK) then message_type_next <= EDP; - stage_next <= TODO; --ACKNACK Processing + stage_next <= PROCESS_ACKNACK; --ACKNACK Processing + -- Initialise Counter + cnt_next <= 1; end if; when ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR => -- SANITY CHECK: Ignore if no Readers @@ -530,7 +542,9 @@ begin -- Only ACKNACKs are relevant if (NUM_READERS /= 0 and opcode = SID_ACKNACK) then message_type_next <= EDP; - stage_next <= TODO; --ACKNACK Processing + stage_next <= PROCESS_ACKNACK; --ACKNACK Processing + -- Initialise counter + cnt_next <= 1; end if; when ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR => -- SANITY CHECK: Ignore if no Writers @@ -558,7 +572,9 @@ begin -- Only ACKNACKs are relevant if (opcode = SID_ACKNACK) then message_type_next <= MESSAGE; - stage_next <= TODO; --ACKNACK Processing + stage_next <= PROCESS_ACKNACK; --ACKNACK Processing + -- Initialise counter + cnt_next <= 1; end if; when ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER => -- Only HEARTBEATs and DATA are relevant (GAPs irrelevant, since depth is 1) @@ -601,7 +617,9 @@ begin -- Only ACKNACKs are relevant if (NUM_WRITERS /= 0 and opcode = SID_ACKNACK) then message_type_next <= EDP; - stage_next <= TODO; --ACKNACK Processing + stage_next <= PROCESS_ACKNACK; --ACKNACK Processing + -- Initialise counter + cnt_next <= 1; end if; when ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER => -- SANITY CHECK: Ignore if no Readers @@ -630,7 +648,9 @@ begin -- Only ACKNACKs are relevant if (NUM_READERS /= 0 and opcode = SID_ACKNACK) then message_type_next <= EDP; - stage_next <= TODO; --ACKNACK Processing + stage_next <= PROCESS_ACKNACK; --ACKNACK Processing + -- Initialise counter + cnt_next <= 1; end if; when ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER => -- SANITY CHECK: Ignore if no Writers @@ -658,7 +678,9 @@ begin -- Only ACKNACKs are relevant if (opcode = SID_ACKNACK) then message_type_next <= MESSAGE; - stage_next <= TODO; --ACKNACK Processing + stage_next <= PROCESS_ACKNACK; --ACKNACK Processing + -- Initialise counter + cnt_next <= 1; end if; when ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER => -- Only HEARTBEATs and DATA are relevant (GAPs irrelevant, since depth is 1) @@ -1362,9 +1384,9 @@ begin end if; when LATCH_BUILTIN_ENDPOINT_QOS => if (empty = '0') then - rd_sig <= '1'; + rd_sig <= '1'; -- Latch QoS - participant_message_best_effort_next <= data_in_swapped(BEST_EFFORT_PARTICIPANT_MESSAGE_DATA_READER); + extra_flags_next <= data_in_swapped; -- DEFAULT Next Stage stage_next <= SKIP_PARAMETER; @@ -1405,14 +1427,14 @@ begin mem_opcode <= UPDATE_PARTICIPANT; -- Update Lease deadline_next <= time + lease_duration; - update_participant_flags_next <= (PARTICIPANT_DATA_FLAG => '1', LEASE_DEADLINE_FLAG => '1', others => '0'); + update_participant_flags_next <= (PARTICIPANT_DATA_FLAG => '1', LEASE_DEADLINE_FLAG => '1', EXTRA_FLAGS_FLAG => '1', others => '0'); start_mem_op <= '1'; -- DONE stage_next <= SKIP_PACKET; end if; -- Endpoint elsif (message_type = EDP) then - -- TODO: Should we renew the Participant Lease on Endpoint Activity? + -- TODO: Should we renew the Participant Lease on Endpoint Activity? - No, it is clearly stated that a participant anouncement has to be received during the lease duration -- Store new Sequence Number mem_opcode <= UPDATE_PARTICIPANT; update_participant_flags_next <= (EDP_SEQ_NR_FLAG => '1', others => '0'); @@ -1607,8 +1629,8 @@ begin if (is_heartbeat_res = '1') then -- If Suppression Delay passed, zero the time if(mem_participant_data.heartbeat_res_time(0) = '1') then - -- Zero Heartbeat Response Time mem_opcode <= UPDATE_PARTICIPANT; + -- Zero Heartbeat Response Time deadline_next <= (others => '0'); update_participant_flags_next <= (HEARTBEAT_RES_TIME_FLAG_FLAG => '1', others => '0'); start_mem_op <= '1'; @@ -1618,9 +1640,15 @@ begin else -- Set Heartbeat Suppression Time mem_opcode <= UPDATE_PARTICIPANT; - deadline_next <= time + TODO; - -- NOTE: Last Bit denotes if this is Response or Suppression Delay - deadline_next(0) <= '1'; + if (TODO /= 0) then + -- Set Heartbeat Suppression Time + deadline_next <= time + TODO; + -- NOTE: Last Bit denotes if this is Response or Suppression Delay + deadline_next(0) <= '1'; + else + -- Zero Heartbeat Response Time + deadline_next <= (others => '0'); + end if; update_participant_flags_next <= (HEARTBEAT_RES_TIME_FLAG_FLAG => '1', others => '0'); start_mem_op <= '1'; -- Send Acknack @@ -1631,8 +1659,8 @@ begin else -- If Suppression Delay passed, zero the time if(mem_participant_data.acknack_res_time(0) = '1') then - -- Zero Acknack Response Time mem_opcode <= UPDATE_PARTICIPANT; + -- Zero Acknack Response Time deadline_next <= (others => '0'); update_participant_flags_next <= (ACKNACK_RES_TIME_FLAG => '1', others => '0'); start_mem_op <= '1'; @@ -1640,16 +1668,26 @@ begin stage_next <= IDLE; -- If Response Delay Passed else - -- Set Acknack Suppression Time mem_opcode <= UPDATE_PARTICIPANT; - deadline_next <= time + TODO; - -- NOTE: Last Bit denotes if this is Response or Suppression Delay - deadline_next(0) <= '1'; - update_participant_flags_next <= (ACKNACK_RES_TIME_FLAG => '1', others => '0'); + if (TODO /= 0) then + -- Set Acknack Suppression Time + deadline_next <= time + TODO; + -- NOTE: Last Bit denotes if this is Response or Suppression Delay + deadline_next(0) <= '1'; + else + -- Zero Acknack Response Time + deadline_next <= (others => '0'); + end if; + -- Zero Data Response Flags + extra_flags_next <= mem_participant_data.extra_flags; + extra_flags_next(PUB_DATA_FLAG) <= '0'; + extra_flags_next(SUB_DATA_FLAG) <= '0'; + extra_flags_next(MES_DATA_FLAG) <= '0'; + update_participant_flags_next <= (EXTRA_FLAGS_FLAG => '1', ACKNACK_RES_TIME_FLAG => '1', others => '0'); start_mem_op <= '1'; - -- Send Acknack - stage_next <= TODO; - cnt_next <= 1; + -- Send Requested Data + stage_next <= SEND_PUB_DATA; + cnt_next <= 0; end if; end if; end if; @@ -1840,7 +1878,6 @@ begin end case; end if; when PROCESS_ACKNACK_SEQUENCE_NUMBERS => - -- NOTE: This guard is not because we need the data from memory, but because we initiate a new memory operation -- Wait for Memory Operation to finish if (mem_done = '1') then -- No scheduled Acknack Response @@ -1849,24 +1886,32 @@ begin when EDP => -- Subscriber Acknack if (is_subscriber = '1') then - -- If reader has not acked all history cache, send data + -- If reader has not acked all Publisher history cache, mark for send if (first_seq_nr <= PUB_SEQUENCE_NR) then mem_opcode <= UPDATE_PARTICIPANT; + -- Set Acknack Response Time deadline_next <= time + TODO; -- NOTE: Last Bit denotes if this is Response or Suppression Delay deadline_next(0) <= '0'; - update_participant_flags_next <= (ACKNACK_RES_TIME_FLAG => '1', others => '0'); + -- Set Publisher Data as Acknack Response + extra_flags_next <= mem_participant_data.extra_flags; + extra_flags_next(PUB_DATA_FLAG) <= '1'; + update_participant_flags_next <= (EXTRA_FLAGS_FLAG => '1', ACKNACK_RES_TIME_FLAG => '1', others => '0'); start_mem_op <= '1'; end if; -- Publisher Acknack else - -- If reader has not acked all history cache, send data + -- If reader has not acked all Subscriber history cache, mark for send if (first_seq_nr <= SUB_SEQUENCE_NR) then mem_opcode <= UPDATE_PARTICIPANT; + -- Set Acknack Response Time deadline_next <= time + TODO; -- NOTE: Last Bit denotes if this is Response or Suppression Delay deadline_next(0) <= '0'; - update_participant_flags_next <= (ACKNACK_RES_TIME_FLAG => '1', others => '0'); + -- Set Subscriber Data as Acknack Response + extra_flags_next <= mem_participant_data.extra_flags; + extra_flags_next(SUB_DATA_FLAG) <= '1'; + update_participant_flags_next <= (EXTRA_FLAGS_FLAG => '1', ACKNACK_RES_TIME_FLAG => '1', others => '0'); start_mem_op <= '1'; end if; end if; @@ -1874,23 +1919,289 @@ begin when MESSAGE => -- NOTE: "auto_live_seq_nr" always has the higher sequence number by design, so we just need to -- check against that - -- If reader has not acked all history cache, send data + -- If reader has not acked all Message history cache, mark for send if (first_seq_nr <= auto_live_seq_nr) then mem_opcode <= UPDATE_PARTICIPANT; + -- Set Acknack Response Time deadline_next <= time + TODO; -- NOTE: Last Bit denotes if this is Response or Suppression Delay deadline_next(0) <= '0'; - update_participant_flags_next <= (ACKNACK_RES_TIME_FLAG => '1', others => '0'); + -- Set Message Data as Acknack Response + extra_flags_next <= mem_participant_data.extra_flags; + extra_flags_next(MES_DATA_FLAG) <= '1'; + update_participant_flags_next <= (EXTRA_FLAGS_FLAG => '1', ACKNACK_RES_TIME_FLAG => '1', others => '0'); start_mem_op <= '1'; end if; end case; + -- Currently in Acknack Response Delay + elsif (mem_participant_data.acknack_res_time(0) = '0') then + case (message_type) is + when EDP => + -- Subscriber Acknack + if (is_subscriber = '1') then + -- Publisher Data not scheduled for response + if (mem_participant_data.extra_flags(PUB_DATA_FLAG)) then + -- If reader has not acked all Publisher history cache, mark for send + if (first_seq_nr <= PUB_SEQUENCE_NR) then + mem_opcode <= UPDATE_PARTICIPANT; + -- Set Publisher Data as Acknack Response + extra_flags_next <= mem_participant_data.extra_flags; + extra_flags_next(PUB_DATA_FLAG) <= '1'; + update_participant_flags_next <= (EXTRA_FLAGS_FLAG => '1', others => '0'); + start_mem_op <= '1'; + end if; + end if; + -- Publisher Acknack + else + -- Subscriber Data not scheduled for response + if (mem_participant_data.extra_flags(SUB_DATA_FLAG)) then + -- If reader has not acked all Subscriber history cache, mark for send + if (first_seq_nr <= SUB_SEQUENCE_NR) then + mem_opcode <= UPDATE_PARTICIPANT; + -- Set Subscriber Data as Acknack Response + extra_flags_next <= mem_participant_data.extra_flags; + extra_flags_next(SUB_DATA_FLAG) <= '1'; + update_participant_flags_next <= (EXTRA_FLAGS_FLAG => '1', others => '0'); + start_mem_op <= '1'; + end if; + end if; + -- Message Acknack + when MESSAGE => + -- Message Data not scheduled for response + if (mem_participant_data.extra_flags(MES_DATA_FLAG)) then + -- NOTE: "auto_live_seq_nr" always has the higher sequence number by design, so we just need to + -- check against that + -- If reader has not acked all Message history cache, mark for send + if (first_seq_nr <= auto_live_seq_nr) then + mem_opcode <= UPDATE_PARTICIPANT; + -- Set Message Data as Acknack Response + extra_flags_next <= mem_participant_data.extra_flags; + extra_flags_next(MES_DATA_FLAG) <= '1'; + update_participant_flags_next <= (EXTRA_FLAGS_FLAG => '1', others => '0'); + start_mem_op <= '1'; + end if; + end if; + end case; end if; -- Done stage_next <= SKIP_PACKET; 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 + stage_next <= SEND_SUB_DATA; + end if; + + if (rtps_full = '0') then + wr_sig <= '1'; + --Increment Counter + cnt_next <= cnt + 1; + -- Send Data + output_sig <= WRITER_ENDPOINT_DATA.data(cnt); + -- Exit Condition + if (cnt = (WRITER_ENDPOINT_DATA.length-1)) then + last_word_out <= '1'; + cnt_next <= 0; + stage_next <= SEND_SUB_DATA; + 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; + end if; + + if (rtps_full = '0') then + wr_sig <= '1'; + --Increment Counter + cnt_next <= cnt + 1; + -- Send Data + output_sig <= READER_ENDPOINT_DATA.data(cnt); + -- Exit Condition + if (cnt = (READER_ENDPOINT_DATA.length-1)) then + last_word_out <= '1'; + cnt_next <= 1; + stage_next <= SEND_MES_DATA; + 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; + end if; + + if (rtps_full = '0') then + wr_sig <= '1'; + --Increment Counter + cnt_next <= cnt + 1; + + case (cnt) is + -- OUTPUT HEADER + -- Src IPv4 Address + when 1 => + output_sig <= DEFAULT_IPv4_MULTICAST_ADDRESS; + -- Dest IPv4 Address + when 2 => + output_sig <= mem_participant_data.meta_addr; + -- Src and Dest UDPv4 Ports + when 3 => + output_sig <= META_IPv4_UNICAST_PORT & mem_participant_data.meta_port; + -- RTPS MESSAGE HEADER + when 4 => + output_sig <= PROTOCOL_RTPS; + when 5 => + output_sig <= PROTOCOLVERSION_2_4 & VENDORID; + when 6 => + output_sig <= GUIDPREFIX(0); + when 7 => + output_sig <= GUIDPREFIX(1); + when 8 => + output_sig <= GUIDPREFIX(2); + cnt_next <= 1; + stage_next <= SEND_MAN_LIVE; + end case; + end if; + when SEND_MES_MAN_LIVE => + if (rtps_full = '0') then + wr_sig <= '1'; + --Increment Counter + cnt_next <= cnt + 1; + + case (cnt) is + -- DATA RTPS SUBMESSAGE (Participant Message) + -- RTPS Submessage Header + when 1 => + output_sig <= SID_DATA & "00000100" & std_logic_vector(to_unsigned(44, 16)); + -- ExtraFlags, octetsToInlineQoS + when 2 => + output_sig <= x"0000" & std_logic_vector(to_unsigned(16, 16)); + -- Reader Entity ID + when 3 => + output_sig <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER; + -- Writer Entity ID + when 4 => + output_sig <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER; + -- Sequence Number 1/2 + when 5 => + output_sig <= man_live_seq_nr(0); + -- Sequence Number 2/2 + when 6 => + output_sig <= man_live_seq_nr(1); + -- Serialized Payload Header + when 7 => + output_sig <= CDR_BE & x"0000"; + -- Serialized Payload BEGIN + -- GUID Prefix 1/3 + when 8 => + output_sig <= GUIDPREFIX(0); + -- GUID Prefix 2/3 + when 9 => + output_sig <= GUIDPREFIX(1); + -- GUID Prefix 3/3 + when 10 => + output_sig <= GUIDPREFIX(3); + -- Participant Message Kind + when 11 => + output_sig <= PARTICIPANT_MESSAGE_DATA_KIND_MANUAL_LIVELINESS_UPDATE; + -- Data Length + when 12 => + output_sig <= (others => '0'); + -- If manual and automatic sequence numbers are not consecutive, we need to send a GAP Message + if (live_gap_start /= auto_live_seq_nr) then + stage_next <= SEND_MES_GAP; + cnt_next <= 1; + -- Else Continue with Automatic Liveliness + else + stage_next <= SEND_MES_AUTO_LIVE; + cnt_next <= 1; + end if; + end case; + end if; + when SEND_MES_GAP => + if (rtps_full = '0') then + wr_sig <= '1'; + --Increment Counter + cnt_next <= cnt + 1; + + case (cnt) is + -- DATA RTPS SUBMESSAGE (Participant Message) + -- RTPS Submessage Header + when 1 => + output_sig <= SID_GAP & "00000000" & std_logic_vector(to_unsigned(28, 16)); + -- Reader Entity ID + when 2 => + output_sig <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER; + -- Writer Entity ID + when 3 => + output_sig <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER; + -- GAP Start Sequence Number 1/2 + when 4 => + output_sig <= live_gap_start(0); + -- GAP Start Sequence Number 2/2 + when 5 => + output_sig <= live_gap_start(1); + -- GAP End Sequence Number Set (Bitmap Base 1/2) + when 6 => + output_sig <= live_gap_end(0); + -- GAP End Sequence Number Set (Bitmap Base 2/2) + when 7 => + output_sig <= live_gap_end(1); + -- GAP End Sequence Number Set (NumBits) + when 8 => + output_sig <= (others => '0'); + stage_next <= SEND_MES_AUTO_LIVE; + cnt_next <= 1; + end case; + end if; + when SEND_MES_AUTO_LIVE => + if (rtps_full = '0') then + wr_sig <= '1'; + --Increment Counter + cnt_next <= cnt + 1; + + case (cnt) is + -- DATA RTPS SUBMESSAGE (Participant Message) + -- RTPS Submessage Header + when 1 => + output_sig <= SID_DATA & "00000100" & std_logic_vector(to_unsigned(44, 16)); + -- ExtraFlags, octetsToInlineQoS + when 2 => + output_sig <= x"0000" & std_logic_vector(to_unsigned(16, 16)); + -- Reader Entity ID + when 3 => + output_sig <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER; + -- Writer Entity ID + when 4 => + output_sig <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER; + -- Sequence Number 1/2 + when 5 => + output_sig <= auto_live_seq_nr(0); + -- Sequence Number 2/2 + when 6 => + output_sig <= auto_live_seq_nr(1); + -- Serialized Payload Header + when 7 => + output_sig <= CDR_BE & x"0000"; + -- Serialized Payload BEGIN + -- GUID Prefix 1/3 + when 8 => + output_sig <= GUIDPREFIX(0); + -- GUID Prefix 2/3 + when 9 => + output_sig <= GUIDPREFIX(1); + -- GUID Prefix 3/3 + when 10 => + output_sig <= GUIDPREFIX(3); + -- Participant Message Kind + when 11 => + output_sig <= PARTICIPANT_MESSAGE_DATA_KIND_AUTOMATIC_LIVELINESS_UPDATE; + -- Data Length + when 12 => + output_sig <= (others => '0'); + last_word_out <= '1'; + stage_next <= IDLE; + end case; + end if; --############################# when SKIP_PARAMETER => -- End of Parameter @@ -2016,12 +2327,15 @@ begin mem_addr_next <= addr_res + 3; mem_cnt_next <= 1; elsif (update_participant_flags(LEASE_DEADLINE_FLAG) = '1') then - mem_addr_next <= addr_res + 11; - mem_cnt_next <= 9; - elsif (update_participant_flags(HEARTBEAT_RES_TIME_FLAG) = '1') then + mem_addr_next <= addr_res + 10; + mem_cnt_next <= 8; + elsif (update_participant_flags(EXTRA_FLAGS_FLAG) = '1') then + mem_addr_next <= addr_res + 12; + mem_cnt_next <= 10; + elsif (update_participant_flags(ACKNACK_RES_TIME_FLAG) = '1') then mem_addr_next <= addr_res + 13; mem_cnt_next <= 11; - elsif (update_participant_flags(ACKNACK_RES_TIME_FLAG) = '1') then + elsif (update_participant_flags(HEARTBEAT_RES_TIME_FLAG) = '1') then mem_addr_next <= addr_res + 15; mem_cnt_next <= 13; elsif (update_participant_flags(EDP_SEQ_NR_FLAG) = '1') then @@ -2101,38 +2415,38 @@ begin mem_participant_data_next.meta_port <= mem_read_data(31 downto 16); mem_participant_data_next.def_port <= mem_read_data(15 downto 0); when 4 => - -- Extra Flags - mem_participant_data_next.extra_flags <= mem_read_data; - when 5 => -- SPDP Sequence Number 1/2 mem_participant_data_next.spdp_seq_nr(0) <= unsigned(mem_read_data); - when 6 => + when 5 => -- SPDP Sequence Number 2/2 mem_participant_data_next.spdp_seq_nr(1) <= unsigned(mem_read_data); - when 7 => + when 6 => -- Lease Duration 1/2 mem_participant_data_next.lease_duration(0) <= unsigned(mem_read_data); - when 8 => + when 7 => -- Lease Duration 2/2 mem_participant_data_next.lease_duration(1) <= unsigned(mem_read_data); - when 9 => + when 8 => -- Lease Deadline 1/2 mem_participant_data_next.lease_deadline(0) <= unsigned(mem_read_data); - when 10 => + when 9 => -- Lease Deadline 2/2 mem_participant_data_next.lease_deadline(1) <= unsigned(mem_read_data); + when 10 => + -- Extra Flags + mem_participant_data_next.extra_flags <= mem_read_data; when 11 => - -- HEARTBEAT DEADLINE 1/2 - mem_participant_data_next.heartbeat_res_time(0) <= unsigned(mem_read_data); - when 12 => - -- HEARTBEAT DEADLINE 2/2 - mem_participant_data_next.heartbeat_res_time(1) <= unsigned(mem_read_data); - when 13 => -- ACKNACK DEADLINE 1/2 mem_participant_data_next.acknack_res_time(0) <= unsigned(mem_read_data); - when 14 => + when 12 => -- ACKNACK DEADLINE 2/2 mem_participant_data_next.acknack_res_time(1) <= unsigned(mem_read_data); + when 13 => + -- HEARTBEAT DEADLINE 1/2 + mem_participant_data_next.heartbeat_res_time(0) <= unsigned(mem_read_data); + when 14 => + -- HEARTBEAT DEADLINE 2/2 + mem_participant_data_next.heartbeat_res_time(1) <= unsigned(mem_read_data); when 15 => -- Publication Sequence Number 1/2 mem_participant_data_next.pub_seq_nr(0) <= unsigned(mem_read_data); @@ -2220,6 +2534,7 @@ begin -- Default Address Increment mem_addr_next <= mem_addr + 1; -- Latch Endpoint Bitmask + -- TODO: Use a different integer ranged in the smalled possible range? endpoint_mask_array_next(mem_cnt) <= mem_read_data; -- Exit Condition if (mem_cnt = ENDPOINT_BITMASK_SIZE-1) then @@ -2377,38 +2692,38 @@ begin -- UDPv4 Ports mem_write_data <= port_latch_2 & port_latch_1; when 7 => - -- Extra Flags - mem_write_data <= (0 => expects_inline_qos, others => '0'); - when 8 => -- SPDP Sequence Number 1/2 mem_write_data <= std_logic_vector(seq_nr(0)); - when 9 => + when 8 => -- SPDP Sequence Number 2/2 mem_write_data <= std_logic_vector(seq_nr(1)); - when 10 => + when 9 => -- Lease Duration 1/2 mem_write_data <= std_logic_vector(lease_duration(0)); - when 11 => + when 10 => -- Lease Duration 2/2 mem_write_data <= std_logic_vector(lease_duration(1)); - when 12 => + when 11 => -- Lease Deadline 1/2 mem_write_data <= std_logic_vector(lease_deadline(0)); - when 13 => + when 12 => -- Lease Deadline 2/2 mem_write_data <= std_logic_vector(lease_deadline(1)); + when 13 => + -- Extra Flags + mem_write_data <= extra_flags; when 14 => - -- HEARTBEAT DEADLINE 1/2 - mem_write_data <= (others => '0'); - when 15 => - -- HEARTBEAT DEADLINE 2/2 - mem_write_data <= (others => '0'); - when 16 => -- ACKNACK DEADLINE 1/2 mem_write_data <= (others => '0'); - when 17 => + when 15 => -- ACKNACK DEADLINE 2/2 mem_write_data <= (others => '0'); + when 16 => + -- HEARTBEAT DEADLINE 1/2 + mem_write_data <= (others => '0'); + when 17 => + -- HEARTBEAT DEADLINE 2/2 + mem_write_data <= (others => '0'); when 18 => -- Publication Sequence Number 1/2 mem_write_data <= (others => '0'); @@ -2640,85 +2955,89 @@ begin mem_wr <= '1'; end if; when 4 => - -- Extra Flags - mem_write_data <= (0 => expects_inline_qos, others => '0'); - if (update_participant_flags(PARTICIPANT_DATA_FLAG) = '1') then - mem_wr <= '1'; - end if; - when 5 => -- SPDP Sequence Number 1/2 mem_write_data <= std_logic_vector(seq_nr(0)); if (update_participant_flags(PARTICIPANT_DATA_FLAG) = '1') then mem_wr <= '1'; end if; - when 6 => + when 5 => -- SPDP Sequence Number 2/2 mem_write_data <= std_logic_vector(seq_nr(1)); if (update_participant_flags(PARTICIPANT_DATA_FLAG) = '1') then mem_wr <= '1'; end if; - when 7 => + when 6 => -- Lease Duration 1/2 mem_write_data <= std_logic_vector(lease_duration(0)); if (update_participant_flags(PARTICIPANT_DATA_FLAG) = '1') then mem_wr <= '1'; end if; - when 8 => + when 7 => -- Lease Duration 2/2 mem_write_data <= std_logic_vector(lease_duration(1)); if (update_participant_flags(PARTICIPANT_DATA_FLAG) = '1') then mem_wr <= '1'; end if; -- If nothing else to update, skip - if (update_participant_flags(4 downto 1) = (4 downto 1 => '0')) then + if (update_participant_flags(5 downto 1) = (5 downto 1 => '0')) then mem_stage_next <= IDLE; end if; - when 9 => + when 8 => -- Lease Deadline 1/2 mem_write_data <= std_logic_vector(lease_deadline(0)); if (update_participant_flags(LEASE_DEADLINE_FLAG) = '1') then mem_wr <= '1'; end if; - when 10 => + when 9 => -- Lease Deadline 2/2 mem_write_data <= std_logic_vector(lease_deadline(1)); if (update_participant_flags(LEASE_DEADLINE_FLAG) = '1') then mem_wr <= '1'; end if; -- If nothing else to update, skip - if (update_participant_flags(4 downto 2) = (4 downto 2 => '0')) then + if (update_participant_flags(5 downto 2) = (5 downto 2 => '0')) then mem_stage_next <= IDLE; end if; - when 11 => - -- HEARTBEAT DEADLINE 1/2 - mem_write_data <= std_logic_vector(heartbeat_res_time(0)); - if (update_participant_flags(HEARTBEAT_RES_TIME_FLAG) = '1') then - mem_wr <= '1'; - end if; - when 12 => - -- HEARTBEAT DEADLINE 2/2 - mem_write_data <= std_logic_vector(heartbeat_res_time(1)); - if (update_participant_flags(HEARTBEAT_RES_TIME_FLAG) = '1') then + when 10 => + -- Extra Flags + mem_write_data <= extra_flags; + if (update_participant_flags(PARTICIPANT_DATA_FLAG) = '1') then mem_wr <= '1'; end if; -- If nothing else to update, skip - if (update_participant_flags(4 downto 3) = (4 downto 3 => '0')) then + if (update_participant_flags(5 downto 3) = (5 downto 3 => '0')) then mem_stage_next <= IDLE; end if; - when 13 => + when 11 => -- ACKNACK DEADLINE 1/2 mem_write_data <= std_logic_vector(acknack_res_time(0)); if (update_participant_flags(ACKNACK_RES_TIME_FLAG) = '1') then mem_wr <= '1'; end if; - when 14 => + when 12 => -- ACKNACK DEADLINE 2/2 mem_write_data <= std_logic_vector(acknack_res_time(1)); if (update_participant_flags(ACKNACK_RES_TIME_FLAG) = '1') then mem_wr <= '1'; end if; -- If nothing else to update, skip - if (update_participant_flags(4 downto 4) = (4 downto 4 => '0')) then + if (update_participant_flags(5 downto 4) = (5 downto 4 => '0')) then + mem_stage_next <= IDLE; + end if; + when 13 => + -- HEARTBEAT DEADLINE 1/2 + mem_write_data <= std_logic_vector(heartbeat_res_time(0)); + if (update_participant_flags(HEARTBEAT_RES_TIME_FLAG) = '1') then + mem_wr <= '1'; + end if; + when 14 => + -- HEARTBEAT DEADLINE 2/2 + mem_write_data <= std_logic_vector(heartbeat_res_time(1)); + if (update_participant_flags(HEARTBEAT_RES_TIME_FLAG) = '1') then + mem_wr <= '1'; + end if; + -- If nothing else to update, skip + if (update_participant_flags(5 downto 5) = (5 downto 5 => '0')) then mem_stage_next <= IDLE; end if; when 15 => diff --git a/src/rtps_package.vhd b/src/rtps_package.vhd index f667f67..98c4e78 100644 --- a/src/rtps_package.vhd +++ b/src/rtps_package.vhd @@ -138,6 +138,7 @@ package rtps_package is constant PAYLOAD_REPRESENTATION_ID : natural := 16; constant PAYLOAD_REPRESENTATION_OPTIONS : natural := 16; constant SEQUENCE_NR_WIDTH : natural := 64; + constant PARTICIPANT_MESSAGE_KIND_WIDTH : natural := 32; -- 'RTPS' in Ascii code constant PROTOCOL_RTPS : std_logic_vector(PROTOCOL_WIDTH-1 downto 0) := x"52545053"; @@ -318,8 +319,14 @@ package rtps_package is constant DISC_BUILTIN_ENDPOINT_TOPICS_DETECTOR : natural := 29; -- BUILTIN ENDPOINT QOS BITMASK + -- XXX: We use some of the unused bits of the 32-bit bitmask when stored in the buffer for internal purposes. (see "builtin_endpoint" Entity) constant BEST_EFFORT_PARTICIPANT_MESSAGE_DATA_READER : natural := 0; + -- PARTICIPANT MESSAGE KIND + constant PARTICIPANT_MESSAGE_DATA_KIND_UNKNOWN : std_logic_vector(PARTICIPANT_MESSAGE_KIND_WIDTH-1 downto 0) := (others => '0'); + constant PARTICIPANT_MESSAGE_DATA_KIND_AUTOMATIC_LIVELINESS_UPDATE : std_logic_vector(PARTICIPANT_MESSAGE_KIND_WIDTH-1 downto 0) := x"00000001"; + constant PARTICIPANT_MESSAGE_DATA_KIND_MANUAL_LIVELINESS_UPDATE : std_logic_vector(PARTICIPANT_MESSAGE_KIND_WIDTH-1 downto 0) := x"00000002"; + --*****CUSTOM***** constant PARTICIPANT_FRAME_SIZE : natural := 19; constant ENDPOINT_BITMASK_SIZE : natural := round_div(MAX_ENDPOINTS, 32); @@ -468,6 +475,10 @@ package body rtps_package is begin ret.data := (others => (others => '0')); ret.length := 0; + -- Sanity Check + if (NUM_READERS = 0) then + return ret; + end if; len := 0; ind := 0; ind2 := 0; @@ -620,6 +631,10 @@ package body rtps_package is begin ret.data := (others => (others => '0')); ret.length := 0; + -- Sanity Check + if (NUM_WRITERS = 0) then + return ret; + end if; len := 0; ind := 0; ind2 := 0;