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

* Heartbeat sending

Browse Source 
* Liveliness Assertion
* Participant Announcement
* Colapse all Header sending into single stage, remove RTPS header from pre-generated data
This commit is contained in:
Greek 2020-10-22 14:56:13 +02:00
parent 7dd316bbdd
commit 73dbc87c08
2 changed files with 323 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

392
src/rtps_builtin_endpoint.vhd
View File

@ -81,6 +81,7 @@ architecture arch of rtps_builtin_endpoint is
constant MES_DATA_FLAG : natural := 29; constant MES_DATA_FLAG : natural := 29;
constant PUB_SEQUENCE_NR : DOUBLE_WORD_ARRAY := convert_to_double_word(to_unsigned(NUM_WRITERS, 64)); 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 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 := ( constant ZERO_PARTICIPANT_DATA : PARTICIPANT_DATA_TYPE := (
meta_addr => (others => '0'), meta_addr => (others => '0'),
def_addr => (others => '0'), def_addr => (others => '0'),
@ -100,7 +101,8 @@ architecture arch of rtps_builtin_endpoint is
--*****SIGNAL DECLARATION***** --*****SIGNAL DECLARATION*****
-- FSM state -- 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; signal mem_stage, mem_stage_next : MEM_STAGE_TYPE := TODO;
-- Intermediate input read signal. (Read from output port not allowed) -- Intermediate input read signal. (Read from output port not allowed)
signal rd_sig : std_logic := '0'; 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_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 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 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 begin
--DEFAULT --DEFAULT
stage_next <= stage; stage_next <= stage;
return_stage_next <= return_stage;
packet_length_next <= packet_length; packet_length_next <= packet_length;
rd_sig <= '0'; rd_sig <= '0';
reset_read_cnt <= '0'; reset_read_cnt <= '0';
@ -400,6 +406,9 @@ begin
man_live_seq_nr_next <= man_live_seq_nr; man_live_seq_nr_next <= man_live_seq_nr;
live_gap_start_next <= live_gap_start; live_gap_start_next <= live_gap_start;
live_gap_end_next <= live_gap_end; 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 -- Last Word Latch Setter
if (last_word_in = '1') then if (last_word_in = '1') then
@ -410,21 +419,44 @@ begin
end if; end if;
-- TODO: Reset Latches -- 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 case(stage) is
-- Initial/Idle State -- Initial/Idle State
when IDLE => when IDLE =>
-- No Packets to process -- Participant Announcement Trigger
if (empty = '1') then if (time > announcement_time) then
mem_opcode <= FIND_STALE_PARTICIPANT; announcement_time_next <= time + TODO;
start_mem_op <= '1'; stage_next <= SEND_PARTICIPANT_ANNOUNCEMENT_1;
stale_check_next <= '1'; -- Heartbeat Trigger
stage_next <= STALE_CHECK; elsif (time > heartbeat_time) then
elsif (stale_check = '0') 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; mem_opcode <= FIND_STALE_PARTICIPANT;
start_mem_op <= '1'; start_mem_op <= '1';
stale_check_next <= '1'; stale_check_next <= '1';
stage_next <= STALE_CHECK; stage_next <= STALE_CHECK;
end if;
else else
-- Process Packet -- Process Packet
stale_check_next <= '0'; stale_check_next <= '0';
@ -1768,7 +1800,8 @@ begin
update_participant_flags_next <= (HEARTBEAT_RES_TIME_FLAG_FLAG => '1', others => '0'); update_participant_flags_next <= (HEARTBEAT_RES_TIME_FLAG_FLAG => '1', others => '0');
start_mem_op <= '1'; start_mem_op <= '1';
-- Send Acknack -- Send Acknack
stage_next <= SEND_ACKNACK; stage_next <= SEND_HEADER;
return_stage_next <= SEND_ACKNACK;
cnt_next <= 0; cnt_next <= 0;
end if; end if;
-- Acknack Response Time -- Acknack Response Time
@ -1802,7 +1835,7 @@ begin
update_participant_flags_next <= (EXTRA_FLAGS_FLAG => '1', ACKNACK_RES_TIME_FLAG => '1', others => '0'); update_participant_flags_next <= (EXTRA_FLAGS_FLAG => '1', ACKNACK_RES_TIME_FLAG => '1', others => '0');
start_mem_op <= '1'; start_mem_op <= '1';
-- Send Requested Data -- Send Requested Data
stage_next <= SEND_PUB_DATA; stage_next <= SEND_DATA;
cnt_next <= 0; cnt_next <= 0;
end if; end if;
end if; end if;
@ -1885,98 +1918,171 @@ begin
cnt_next <= cnt + 1; cnt_next <= cnt + 1;
case (cnt) is 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) -- ACKNACK RTPS SUBMESSAGE (Publication)
-- RTPS Submessage Header -- RTPS Submessage Header
when 8 => when 0 =>
output_sig <= SID_ACKNACK & "00000010" & std_logic_vector(to_unsigned(24, 16)); output_sig <= SID_ACKNACK & "00000010" & std_logic_vector(to_unsigned(24, 16));
-- Reader Entity ID -- Reader Entity ID
when 9 => when 1 =>
output_sig <= ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR; output_sig <= ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR;
-- Writer Entity ID -- Writer Entity ID
when 10 => when 2 =>
output_sig <= ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER; output_sig <= ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER;
-- Sequence Number Set (Bitmap Base 1/2) -- Sequence Number Set (Bitmap Base 1/2)
when 11 => when 3 =>
output_sig <= mem_participant_data.pub_seq_nr(0); output_sig <= mem_participant_data.pub_seq_nr(0);
-- Sequence Number Set (Bitmap Base 2/2) -- Sequence Number Set (Bitmap Base 2/2)
when 12 => when 4 =>
output_sig <= mem_participant_data.pub_seq_nr(1); output_sig <= mem_participant_data.pub_seq_nr(1);
-- Sequence Number Set (NumBits) -- Sequence Number Set (NumBits)
when 13 => when 5 =>
output_sig <= (others => '0'); output_sig <= (others => '0');
-- Count -- Count
when 14 => when 6 =>
output_sig <= count; output_sig <= count;
-- ACKNACK RTPS SUBMESSAGE (Subscription) -- ACKNACK RTPS SUBMESSAGE (Subscription)
-- RTPS Submessage Header -- RTPS Submessage Header
when 15 => when 7 =>
output_sig <= SID_ACKNACK & "00000010" & std_logic_vector(to_unsigned(24, 16)); output_sig <= SID_ACKNACK & "00000010" & std_logic_vector(to_unsigned(24, 16));
-- Reader Entity ID -- Reader Entity ID
when 16 => when 8 =>
output_sig <= ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR; output_sig <= ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR;
-- Writer Entity ID -- Writer Entity ID
when 17 => when 9 =>
output_sig <= ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER; output_sig <= ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER;
-- Sequence Number Set (Bitmap Base 1/2) -- Sequence Number Set (Bitmap Base 1/2)
when 18 => when 10 =>
output_sig <= mem_participant_data.sub_seq_nr(0); output_sig <= mem_participant_data.sub_seq_nr(0);
-- Sequence Number Set (Bitmap Base 2/2) -- Sequence Number Set (Bitmap Base 2/2)
when 19 => when 11 =>
output_sig <= mem_participant_data.sub_seq_nr(1); output_sig <= mem_participant_data.sub_seq_nr(1);
-- Sequence Number Set (NumBits) -- Sequence Number Set (NumBits)
when 20 => when 12 =>
output_sig <= (others => '0'); output_sig <= (others => '0');
-- Count -- Count
when 21 => when 13 =>
output_sig <= count; output_sig <= count;
-- ACKNACK RTPS SUBMESSAGE (Message) -- ACKNACK RTPS SUBMESSAGE (Message)
-- RTPS Submessage Header -- RTPS Submessage Header
when 22 => when 14 =>
output_sig <= SID_ACKNACK & "00000010" & std_logic_vector(to_unsigned(24, 16)); output_sig <= SID_ACKNACK & "00000010" & std_logic_vector(to_unsigned(24, 16));
-- Reader Entity ID -- Reader Entity ID
when 23 => when 15 =>
output_sig <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER; output_sig <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER;
-- Writer Entity ID -- Writer Entity ID
when 24 => when 16 =>
output_sig <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER; output_sig <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER;
-- Sequence Number Set (Bitmap Base 1/2) -- Sequence Number Set (Bitmap Base 1/2)
when 25 => when 17 =>
output_sig <= mem_participant_data.mes_seq_nr(0); output_sig <= mem_participant_data.mes_seq_nr(0);
-- Sequence Number Set (Bitmap Base 2/2) -- Sequence Number Set (Bitmap Base 2/2)
when 26 => when 18 =>
output_sig <= mem_participant_data.mes_seq_nr(1); output_sig <= mem_participant_data.mes_seq_nr(1);
-- Sequence Number Set (NumBits) -- Sequence Number Set (NumBits)
when 27 => when 19 =>
output_sig <= (others => '0'); output_sig <= (others => '0');
-- Count -- Count
when 28 => when 20 =>
output_sig <= count; output_sig <= count;
-- Signal Last Word7 -- Signal Last Word7
first_seq_nr_next(1) <= data_in; first_seq_nr_next(1) <= data_in;
stage_next <= PROCESS_ACKNACK_SEQUENCE_NUMBERS stage_next <= PROCESS_ACKNACK_SEQUENCE_NUMBERS
end case; end case;
end if; 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 => when PROCESS_ACKNACK_SEQUENCE_NUMBERS =>
-- Wait for Memory Operation to finish -- Wait for Memory Operation to finish
if (mem_done = '1') then if (mem_done = '1') then
@ -2086,6 +2192,70 @@ begin
-- Done -- Done
stage_next <= SKIP_PACKET; stage_next <= SKIP_PACKET;
end if; 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 => when SEND_PUB_DATA =>
-- If Publisher Data not scheduled for response or no Writers available, skip -- 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 if (mem_participant_data.extra_flags(PUB_DATA_FLAG) = '0' or NUM_WRITERS = 0) then
@ -2099,14 +2269,19 @@ begin
-- Exit Condition -- Exit Condition
if (cnt = (WRITER_ENDPOINT_DATA.length-1)) then if (cnt = (WRITER_ENDPOINT_DATA.length-1)) then
last_word_out <= '1'; last_word_out <= '1';
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; cnt_next <= 0;
stage_next <= SEND_SUB_DATA; else
stage_next <= IDLE;
end if;
end if; end if;
end if; end if;
when SEND_SUB_DATA => when SEND_SUB_DATA =>
-- If Subscriber Data not scheduled for response or no Readers available, skip -- 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 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 elsif (rtps_full = '0') then
wr_sig <= '1'; wr_sig <= '1';
--Increment Counter --Increment Counter
@ -2116,44 +2291,14 @@ begin
-- Exit Condition -- Exit Condition
if (cnt = (READER_ENDPOINT_DATA.length-1)) then if (cnt = (READER_ENDPOINT_DATA.length-1)) then
last_word_out <= '1'; last_word_out <= '1';
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; cnt_next <= 0;
stage_next <= SEND_MES_DATA; else
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; stage_next <= IDLE;
elsif (rtps_full = '0') then end if;
wr_sig <= '1'; end if;
--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; end if;
when SEND_MES_MAN_LIVE => when SEND_MES_MAN_LIVE =>
if (rtps_full = '0') then if (rtps_full = '0') then
@ -2293,7 +2438,15 @@ begin
when 11 => when 11 =>
output_sig <= (others => '0'); output_sig <= (others => '0');
last_word_out <= '1'; last_word_out <= '1';
-- 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; stage_next <= IDLE;
end if;
end case; end case;
end if; end if;
when LIVELINESS_UPDATE => when LIVELINESS_UPDATE =>
@ -2436,6 +2589,17 @@ begin
mem_addr_base_next <= tmp; mem_addr_base_next <= tmp;
mem_addr_next <= tmp; mem_addr_next <= tmp;
mem_cnt_next <= 0; 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 => when UPDATE_PARTICIPANT =>
mem_stage_next <= UPDATE_PARTICIPANT; mem_stage_next <= UPDATE_PARTICIPANT;
if (update_participant_flags(PARTICIPANT_DATA_FLAG) = '1') then if (update_participant_flags(PARTICIPANT_DATA_FLAG) = '1') then
@ -2932,6 +3096,54 @@ begin
mem_cnt_next <= 0; mem_cnt_next <= 0;
endpoint_mask_array_next <= convert_to_bitmask_array(endpoint_mask); endpoint_mask_array_next <= convert_to_bitmask_array(endpoint_mask);
end case; 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 => when FIND_STALE_PARTICIPANT =>
mem_rd <= '1'; mem_rd <= '1';
-- Increment counter -- Increment counter

47
src/rtps_package.vhd
View File

@ -468,8 +468,8 @@ package body rtps_package is
end function; end function;
function gen_reader_endpoint_data return OUTPUT_DATA_TYPE is function gen_reader_endpoint_data return OUTPUT_DATA_TYPE is
-- Limit DATA to MAX UDPv4 Payload Size (65,507 Bytes) -- Limit DATA to MAX UDPv4 Payload Size - RTPS Header (65487 Bytes)
variable ret : OUTPUT_DATA_TYPE(data(0 to 16376)) := (data => (others => (others => '0')), length => 0); variable ret : OUTPUT_DATA_TYPE(data(0 to 16371)) := (data => (others => (others => '0')), length => 0);
variable ind : natural := 0; variable ind : natural := 0;
variable len : natural := 0; variable len : natural := 0;
variable tmp : natural := 0; variable tmp : natural := 0;
@ -483,13 +483,7 @@ package body rtps_package is
len := 0; len := 0;
ind := 0; ind := 0;
ind2 := 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 -- RTPS Submessages
-- One DATA Submessage for each Reader -- One DATA Submessage for each Reader
for i in 0 to NUM_READERS-1 loop for i in 0 to NUM_READERS-1 loop
@ -526,12 +520,10 @@ package body rtps_package is
len := len + 1; len := len + 1;
ret.data(ind+len) := ENTITYID(i); ret.data(ind+len) := ENTITYID(i);
-- EXPECTS INLINE QOS (Only relevant for Reader endpoints) -- EXPECTS INLINE QOS (Only relevant for Reader endpoints)
if (i < NUM_READERS) then
len := len + 1; len := len + 1;
ret.data(ind+len) := PID_EXPECTS_INLINE_QOS & std_logic_vector(to_unsigned(4, 16)); ret.data(ind+len) := PID_EXPECTS_INLINE_QOS & std_logic_vector(to_unsigned(4, 16));
len := len + 1; len := len + 1;
ret.data(ind+len) := (24 => '1', others => '0'); ret.data(ind+len) := (24 => '1', others => '0');
end if;
-- TOPIC NAME -- TOPIC NAME
tmp := string_len(ENDPOINT_TOPIC(i)); tmp := string_len(ENDPOINT_TOPIC(i));
len := len + 1; len := len + 1;
@ -624,8 +616,8 @@ package body rtps_package is
constant READER_ENDPOINT_DATA : OUTPUT_DATA_TYPE := gen_reader_endpoint_data; constant READER_ENDPOINT_DATA : OUTPUT_DATA_TYPE := gen_reader_endpoint_data;
function gen_writer_endpoint_data return OUTPUT_DATA_TYPE is function gen_writer_endpoint_data return OUTPUT_DATA_TYPE is
-- Limit DATA to MAX UDPv4 Payload Size (65,507 Bytes) -- Limit DATA to MAX UDPv4 Payload Size - RTPS Header (65487 Bytes)
variable ret : OUTPUT_DATA_TYPE(data(0 to 16376)) := (data => (others => (others => '0')), length => 0); variable ret : OUTPUT_DATA_TYPE(data(0 to 16371)) := (data => (others => (others => '0')), length => 0);
variable ind : natural := 0; variable ind : natural := 0;
variable len : natural := 0; variable len : natural := 0;
variable tmp : natural := 0; variable tmp : natural := 0;
@ -639,13 +631,7 @@ package body rtps_package is
len := 0; len := 0;
ind := 0; ind := 0;
ind2 := 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 -- RTPS Submessages
-- One DATA Submessage for each Writer Endpoint -- One DATA Submessage for each Writer Endpoint
for i in NUM_READERS to MAX_ENDPOINTS-1 loop 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); ret.data(ind+len) := GUIDPREFIX(2);
len := len + 1; len := len + 1;
ret.data(ind+len) := ENTITYID(i); 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 -- TOPIC NAME
tmp := string_len(ENDPOINT_TOPIC(i)); tmp := string_len(ENDPOINT_TOPIC(i));
len := len + 1; len := len + 1;
@ -789,8 +768,8 @@ package body rtps_package is
constant ENDPOINT_DATA : OUTPUT_DATA_TYPE := gen_writer_endpoint_data; constant ENDPOINT_DATA : OUTPUT_DATA_TYPE := gen_writer_endpoint_data;
function gen_participant_data return OUTPUT_DATA_TYPE is function gen_participant_data return OUTPUT_DATA_TYPE is
-- Limit DATA to MAX UDPv4 Payload Size (65,507 Bytes) -- Limit DATA to MAX UDPv4 Payload Size - RTPS Header (65487 Bytes)
variable ret : OUTPUT_DATA_TYPE(data(0 to 16376)) := (data => (others => (others => '0')), length => 0); variable ret : OUTPUT_DATA_TYPE(data(0 to 16371)) := (data => (others => (others => '0')), length => 0);
variable ind : natural := 0; variable ind : natural := 0;
variable len : natural := 0; variable len : natural := 0;
variable tmp : natural := 0; variable tmp : natural := 0;
@ -800,13 +779,7 @@ package body rtps_package is
len := 0; len := 0;
ind := 0; ind := 0;
ind2 := 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 DATA SUBMESSAGE
-- RTPS Submessage Header -- RTPS Submessage Header
ret.data(ind) := SID_DATA & "00000100" & x"0000"; ret.data(ind) := SID_DATA & "00000100" & x"0000";
@ -919,7 +892,7 @@ package body rtps_package is
len := len + 1; len := len + 1;
ret.data(ind+len) := LOCATOR_KIND_UDPv4; ret.data(ind+len) := LOCATOR_KIND_UDPv4;
len := len + 1; 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; len := len + 1;
ret.data(ind+len) := (others => '0'); ret.data(ind+len) := (others => '0');
len := len + 1; len := len + 1;