greek64/rtps-fpga
2
1
Fork 0
Code Issues Pull Requests Releases Wiki Activity

* Finished ACKNACK and HEARTBEAT handling

Browse Source
This commit is contained in:
Greek 2020-10-21 16:57:40 +02:00
parent b79e631ac6
commit 69da90be20
2 changed files with 429 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

495
src/rtps_builtin_endpoint.vhd
View File

@ -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)
@ -1364,7 +1386,7 @@ begin
if (empty = '0') then
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;
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;
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';
update_participant_flags_next <= (ACKNACK_RES_TIME_FLAG => '1', others => '0');
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 =>

15
src/rtps_package.vhd
View File

@ -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;