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

Various Fixes in RTPS Reader/Writer

Browse Source 
Amongst other changes the RTPS Reader now supportes VOLATILE Durability
(Will only request most recent change from new matched Writers)
This commit is contained in:
Greek 2021-02-14 19:58:09 +01:00
parent 348d036659
commit a3a51e2f52
3 changed files with 150 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/rtps_package.vhd
View File

@ -105,6 +105,7 @@ package rtps_package is
constant TIME_ZERO : TIME_TYPE := (others => (others => '0')); constant TIME_ZERO : TIME_TYPE := (others => (others => '0'));
constant TIME_INVALID : TIME_TYPE := (others => (others => '1')); constant TIME_INVALID : TIME_TYPE := (others => (others => '1'));
constant TIME_INFINITE : TIME_TYPE := (x"ffffffff", x"fffffffe"); constant TIME_INFINITE : TIME_TYPE := (x"ffffffff", x"fffffffe");
constant FIRST_SEQUENCENUMBER : SEQUENCENUMBER_TYPE := (x"00000000", x"00000001");
constant SEQUENCENUMBER_UNKNOWN : SEQUENCENUMBER_TYPE := (x"ffffffff", x"00000000"); constant SEQUENCENUMBER_UNKNOWN : SEQUENCENUMBER_TYPE := (x"ffffffff", x"00000000");
constant PROTOCOL_RTPS : std_logic_vector(PROTOCOL_WIDTH-1 downto 0) := x"52545053"; -- 'RTPS' in Ascii code constant PROTOCOL_RTPS : std_logic_vector(PROTOCOL_WIDTH-1 downto 0) := x"52545053"; -- 'RTPS' in Ascii code
constant PROTOCOLVERSION_1_0 : std_logic_vector(PROTOCOLVERSION_WIDTH-1 downto 0) := x"0100"; constant PROTOCOLVERSION_1_0 : std_logic_vector(PROTOCOLVERSION_WIDTH-1 downto 0) := x"0100";

42
src/rtps_reader.vhd
View File

@ -13,6 +13,7 @@ entity rtps_reader is
generic ( generic (
RELIABILTY_QOS : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0) := DEFAULT_RELIABILTY_QOS; RELIABILTY_QOS : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0) := DEFAULT_RELIABILTY_QOS;
LIVELINESS_QOS : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0) := DEFAULT_LIVELINESS_QOS; LIVELINESS_QOS : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0) := DEFAULT_LIVELINESS_QOS;
DURABILITY_QOS : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0) := DEFAULT_DURABILITY_QOS;
HEARTBEAT_RESPONSE_DELAY : DURATION_TYPE := TODO; HEARTBEAT_RESPONSE_DELAY : DURATION_TYPE := TODO;
HEARTBEAT_SUPPRESSION_DELAY : DURATION_TYPE := TODO; HEARTBEAT_SUPPRESSION_DELAY : DURATION_TYPE := TODO;
LEASE_DURATION : DURATION_TYPE := DEFAULT_LEASE_DURATION; LEASE_DURATION : DURATION_TYPE := DEFAULT_LEASE_DURATION;
@ -587,7 +588,7 @@ begin
case (opcode) is case (opcode) is
when SID_DATA => when SID_DATA =>
stage_next <= LATCH_EXTRA_DATA; stage_next <= LATCH_EXTRA_DATA;
mem_field_flags <= (others => '0'); mem_field_flags <= EMF_NEXT_SEQ_NR_FLAG;
cnt_next <= 0; cnt_next <= 0;
when SID_HEARTBEAT => when SID_HEARTBEAT =>
stage_next <= LATCH_HEARTBEAT; stage_next <= LATCH_HEARTBEAT;
@ -850,8 +851,23 @@ begin
-- No scheduled Heartbeat Response -- No scheduled Heartbeat Response
if (mem_endpoint_data.res_time = TIME_INVALID) then if (mem_endpoint_data.res_time = TIME_INVALID) then
-- If Reader is Volatile and we have not received anything from the writer yet
if (DURABILITY_QOS = VOLATILE_DURABILITY_QOS and mem_endpoint_data.next_seq_nr = SEQUENCENUMBER_UNKNOWN) then
-- Mark last available SN as next expected (Ignore historical data)
next_seq_nr_next <= last_seq_nr;
mem_op_start <= '1';
mem_opcode <= UPDATE_ENDPOINT;
tmp_flags := tmp_flags or EMF_NEXT_SEQ_NR_FLAG or EMF_RES_TIME_FLAG;
if (HEARTBEAT_RESPONSE_DELAY /= DURATION_INFINITE) then
res_time <= time + HEARTBEAT_RESPONSE_DELAY;
-- NOTE: Last Bit denotes if this is Response or Suppression Delay
res_time(1)(0) <= '0';
tmp_dw := (time + HEARTBEAT_RESPONSE_DELAY) when ((time + HEARTBEAT_RESPONSE_DELAY) < tmp_dw);
else
res_time <= TIME_INVALID;
end if;
-- If current Sequence Number obsolete (removed from source history cache) -- If current Sequence Number obsolete (removed from source history cache)
if (first_seq_nr > mem_endpoint_data.next_seq_nr and first_seq_nr <= last_seq_nr) then elsif (first_seq_nr > mem_endpoint_data.next_seq_nr and first_seq_nr <= last_seq_nr) then
-- Store new expected Sequence Number and set Response Dealy -- Store new expected Sequence Number and set Response Dealy
next_seq_nr_next <= first_seq_nr; next_seq_nr_next <= first_seq_nr;
mem_op_start <= '1'; mem_op_start <= '1';
@ -1146,8 +1162,13 @@ begin
stage_next <= SKIP_PARAMETER; stage_next <= SKIP_PARAMETER;
end if; end if;
when INITIATE_ADD_CACHE_CHANGE_REQUEST => when INITIATE_ADD_CACHE_CHANGE_REQUEST =>
-- SANITY CHECK: Skip if no Hash Key and no payload -- Wait for Endpoint Data
if (mem_op_done = '1') then
-- Data is Next expected Sequence Number
if ((RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and seq_nr = mem_endpoint_data.next_seq_nr) or (RELIABILTY_QOS = BEST_EFFORT_RELIABILITY_QOS and seq_nr >= mem_endpoint_data.next_seq_nr) or (DURABILITY_QOS = VOLATILE_DURABILITY_QOS and mem_endpoint_data.next_seq_nr = SEQUENCENUMBER_UNKNOWN)) then
-- SANITY CHECK: Skip if no Hash Key and no Payload
if (WITH_KEY and key_hash_rcvd = '0' and data_flag = '0' and key_flag = '0') then if (WITH_KEY and key_hash_rcvd = '0' and data_flag = '0' and key_flag = '0') then
-- Ignore
stage_next <= SKIP_PACKET; stage_next <= SKIP_PACKET;
else else
start_hc <= '1'; start_hc <= '1';
@ -1158,6 +1179,11 @@ begin
cnt_next <= 0; cnt_next <= 0;
end if; end if;
end if; end if;
else
-- Ignore
stage_next <= SKIP_PACKET;
end if;
end if;
when ADD_CACHE_CHANGE => when ADD_CACHE_CHANGE =>
case (cnt) is case (cnt) is
-- Status Info -- Status Info
@ -1529,12 +1555,8 @@ begin
-- Reset Parameter End -- Reset Parameter End
parameter_end_next <= (others => '1'); parameter_end_next <= (others => '1');
-- Stale Check Exit
if (stale_check = '1') then
-- DONE
stage_next <= IDLE;
-- Consumed last word of Packet -- Consumed last word of Packet
elsif (last_word_in_latch = '1' and last_word_in_user = '0') then if (last_word_in_latch = '1' and last_word_in_user = '0') then
-- Reset Last Word In Latch -- Reset Last Word In Latch
last_word_in_latch_next <= '0'; last_word_in_latch_next <= '0';
-- DONE -- DONE
@ -2340,7 +2362,7 @@ begin
when 6 => when 6 =>
mem_valid_in <= '1'; mem_valid_in <= '1';
mem_addr <= mem_addr_base + EMF_NEXT_SEQ_NR_OFFSET; mem_addr <= mem_addr_base + EMF_NEXT_SEQ_NR_OFFSET;
mem_write_data <= (others => '0'); mem_write_data <= SEQUENCENUMBER_UNKNOWN(0) when (DURABILITY_QOS = VOLATILE_DURABILITY_QOS) else FIRST_SEQUENCENUMBER(0);
if (mem_ready_in = '1') then if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1; mem_cnt_next <= mem_cnt + 1;
end if; end if;
@ -2349,7 +2371,7 @@ begin
mem_write_data <= (others => '0'); mem_write_data <= (others => '0');
mem_valid_in <= '1'; mem_valid_in <= '1';
mem_addr <= mem_addr_base + EMF_NEXT_SEQ_NR_OFFSET + 1; mem_addr <= mem_addr_base + EMF_NEXT_SEQ_NR_OFFSET + 1;
mem_write_data <= to_unsigned(1,CDR_LONG_WIDTH); mem_write_data <= SEQUENCENUMBER_UNKNOWN(1) when (DURABILITY_QOS = VOLATILE_DURABILITY_QOS) else FIRST_SEQUENCENUMBER(1);
if (mem_ready_in = '1') then if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1; mem_cnt_next <= mem_cnt + 1;
end if; end if;

41
src/rtps_writer.vhd
View File

@ -713,7 +713,7 @@ begin
seq_nr <= SEQUENCENUMBER_UNKNOWN when (reader_flags(BEST_EFFORT_FLAG) = '1') else (others => (others => '0')); seq_nr <= SEQUENCENUMBER_UNKNOWN when (reader_flags(BEST_EFFORT_FLAG) = '1') else (others => (others => '0'));
-- Reader needs Historical Data -- Reader needs Historical Data
if (reader_flags(SEND_HISTORICAL_DATA_FLAG) = '1') then if (DURABILITY_QOS /= VOLATILE_DURABILITY_QOS and reader_flags(SEND_HISTORICAL_DATA_FLAG) = '1') then
-- Send Historical Data -- Send Historical Data
historical_push_next <= '1'; historical_push_next <= '1';
@ -1229,7 +1229,7 @@ begin
min_sn_next(1) <= data_in_hc; min_sn_next(1) <= data_in_hc;
assert (last_word_in_hc = '1') severity FAILURE; assert (last_word_in_hc = '1') severity FAILURE;
if (historical_push = '1') then if (DURABILITY_QOS /= VOLATILE_DURABILITY_QOS and historical_push = '1') then
stage_next <= HANDLE_HISTORICAL; stage_next <= HANDLE_HISTORICAL;
cnt_next <= 4; -- Pre-Check Sub-state cnt_next <= 4; -- Pre-Check Sub-state
elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS or LIVELINESS_QOS = MANUAL_BY_TOPIC_LIVELINESS_QOS) then elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS or LIVELINESS_QOS = MANUAL_BY_TOPIC_LIVELINESS_QOS) then
@ -1501,6 +1501,8 @@ begin
end case; end case;
end if; end if;
when HANDLE_HISTORICAL => when HANDLE_HISTORICAL =>
-- Synthesis Guard
if (DURABILITY_QOS /= VOLATILE_DURABILITY_QOS) then
case (cnt) is case (cnt) is
-- Next SN -- Next SN
when 0 => when 0 =>
@ -1600,6 +1602,7 @@ begin
when others => when others =>
null; null;
end case; end case;
end if;
when SEND_HEADER => when SEND_HEADER =>
-- Synthesis Guard -- Synthesis Guard
if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
@ -1715,15 +1718,15 @@ begin
cnt_next <= 0; cnt_next <= 0;
else else
-- Continue -- Continue
if (stale_check = '1') then if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and stale_check = '1') then
stage_next <= HANDLE_REQUESTS; stage_next <= HANDLE_REQUESTS;
cnt_next <= 0; cnt_next <= 0;
req_bitmap_pos_next <= req_bitmap_pos + 1; req_bitmap_pos_next <= req_bitmap_pos + 1;
next_seq_nr_next <= next_seq_nr + 1; next_seq_nr_next <= next_seq_nr + 1;
elsif (new_push = '1') then elsif (PUSH_MODE and new_push = '1') then
stage_next <= HANDLE_NEW; stage_next <= HANDLE_NEW;
cnt_next <= 4; cnt_next <= 4;
elsif (historical_push = '1') then elsif (DURABILITY_QOS /= VOLATILE_DURABILITY_QOS and historical_push = '1') then
stage_next <= HANDLE_HISTORICAL; stage_next <= HANDLE_HISTORICAL;
cnt_next <= 0; cnt_next <= 0;
else else
@ -1804,15 +1807,15 @@ begin
stage_next <= SEND_DATA_B; stage_next <= SEND_DATA_B;
else else
-- Continue -- Continue
if (stale_check = '1') then if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and stale_check = '1') then
stage_next <= HANDLE_REQUESTS; stage_next <= HANDLE_REQUESTS;
cnt_next <= 0; cnt_next <= 0;
req_bitmap_pos_next <= req_bitmap_pos + 1; req_bitmap_pos_next <= req_bitmap_pos + 1;
next_seq_nr_next <= next_seq_nr + 1; next_seq_nr_next <= next_seq_nr + 1;
elsif (new_push = '1') then elsif (PUSH_MODE and new_push = '1') then
stage_next <= HANDLE_NEW; stage_next <= HANDLE_NEW;
cnt_next <= 4; cnt_next <= 4;
elsif (historical_push = '1') then elsif (DURABILITY_QOS /= VOLATILE_DURABILITY_QOS and historical_push = '1') then
stage_next <= HANDLE_HISTORICAL; stage_next <= HANDLE_HISTORICAL;
cnt_next <= 0; cnt_next <= 0;
else else
@ -1837,15 +1840,15 @@ begin
stage_next <= SEND_DATA_B; stage_next <= SEND_DATA_B;
else else
-- Continue -- Continue
if (stale_check = '1') then if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and stale_check = '1') then
stage_next <= HANDLE_REQUESTS; stage_next <= HANDLE_REQUESTS;
cnt_next <= 0; cnt_next <= 0;
req_bitmap_pos_next <= req_bitmap_pos + 1; req_bitmap_pos_next <= req_bitmap_pos + 1;
next_seq_nr_next <= next_seq_nr + 1; next_seq_nr_next <= next_seq_nr + 1;
elsif (new_push = '1') then elsif (PUSH_MODE and new_push = '1') then
stage_next <= HANDLE_NEW; stage_next <= HANDLE_NEW;
cnt_next <= 4; cnt_next <= 4;
elsif (historical_push = '1') then elsif (DURABILITY_QOS /= VOLATILE_DURABILITY_QOS and historical_push = '1') then
stage_next <= HANDLE_HISTORICAL; stage_next <= HANDLE_HISTORICAL;
cnt_next <= 0; cnt_next <= 0;
else else
@ -1867,15 +1870,15 @@ begin
last_word_out_rtps = '1'; last_word_out_rtps = '1';
-- Continue -- Continue
if (stale_check = '1') then if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and stale_check = '1') then
stage_next <= HANDLE_REQUESTS; stage_next <= HANDLE_REQUESTS;
cnt_next <= 0; cnt_next <= 0;
req_bitmap_pos_next <= req_bitmap_pos + 1; req_bitmap_pos_next <= req_bitmap_pos + 1;
next_seq_nr_next <= next_seq_nr + 1; next_seq_nr_next <= next_seq_nr + 1;
elsif (new_push = '1') then elsif (PUSH_MODE and new_push = '1') then
stage_next <= HANDLE_NEW; stage_next <= HANDLE_NEW;
cnt_next <= 4; cnt_next <= 4;
elsif (historical_push = '1') then elsif (DURABILITY_QOS /= VOLATILE_DURABILITY_QOS and historical_push = '1') then
stage_next <= HANDLE_HISTORICAL; stage_next <= HANDLE_HISTORICAL;
cnt_next <= 0; cnt_next <= 0;
else else
@ -1909,12 +1912,12 @@ begin
data_out <= std_logic_vector(next_seq_nr(1)); data_out <= std_logic_vector(next_seq_nr(1));
-- Continue -- Continue
if (stale_check = '1') then if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and stale_check = '1') then
stage_next <= HANDLE_REQUESTS; stage_next <= HANDLE_REQUESTS;
cnt_next <= 0; cnt_next <= 0;
req_bitmap_pos_next <= req_bitmap_pos + 1; req_bitmap_pos_next <= req_bitmap_pos + 1;
next_seq_nr_next <= next_seq_nr + 1; next_seq_nr_next <= next_seq_nr + 1;
elsif (historical_push = '1') then elsif (DURABILITY_QOS /= VOLATILE_DURABILITY_QOS and historical_push = '1') then
stage_next <= HANDLE_HISTORICAL; stage_next <= HANDLE_HISTORICAL;
cnt_next <= 0; cnt_next <= 0;
end if; end if;
@ -1997,12 +2000,8 @@ begin
-- Reset Parameter End -- Reset Parameter End
parameter_end_next <= (others => '1'); parameter_end_next <= (others => '1');
-- Stale Check Exit
if (stale_check = '1') then
-- DONE
stage_next <= IDLE;
-- Consumed last word of Packet -- Consumed last word of Packet
elsif (last_word_in_latch = '1' and last_word_in_user = '0') then if (last_word_in_latch = '1' and last_word_in_user = '0') then
-- Reset Last Word In Latch -- Reset Last Word In Latch
last_word_in_latch_next <= '0'; last_word_in_latch_next <= '0';
-- DONE -- DONE