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

Fix DESTINATION_ORDER_QOS in dds_endpoint

Browse Source 
The rtps_endpoint only sends the source timestamp (TIME_INVALID if no
source timestamp provided). If ordering BY_RECEPTION, the sample is added
to the list tail. If ordering BY_SOURCE the samples are ordered
according to the source timestamp, but droped if the source timestamp is
earlier than a sample that has been read by the user or INVALID (Ensures
determenistic behavior).
This commit is contained in:
Greek 2021-01-26 18:57:09 +01:00
parent 164bd508c1
commit 728a6964d3
1 changed files with 116 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

161
src/dds_endpoint.vhd
View File

@ -2,9 +2,6 @@ library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
-- TODO: Cancel KHG operation on incomplete payload parsing
-- TODO: Make sure rest of Payload slot is zero initialized
entity history_cache is
generic (
TIME_BASED_FILTER_QOS : DURATION_TYPE := DEFAULT_TIME_BASED_FILTER_QOS;
@ -13,6 +10,7 @@ entity history_cache is
HISTORY_QOS : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0) := DEFAULT_HISTORY_QOS;
RELIABILITY_QOS : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0) := DEFAULT_RELIABILTY_QOS;
PRESENTATION_QOS : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0) := DEFAULT_PRESENTATION_QOS;
DESTINATION_ORDER_QOS : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0) := DEFAULT_DESTINATION_ORDER_QOS;
COHERENT_ACCESS : boolean := DEFAULT_COHERENT_ACCESS;
ORDERED_ACCESS : boolean := DEFAULT_ORDERED_ACCESS;
);
@ -206,6 +204,7 @@ architecture arch of history_cache is
signal sample_addr_latch_2, sample_addr_latch_2_next : unsigned(SAMPLE_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0');
signal sample_addr_latch_3, sample_addr_latch_3_next : unsigned(SAMPLE_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0');
signal ts_latch, ts_latch_next : TIME_TYPE := TIME_INVALID;
signal last_read_ts, last_read_ts_next : TIME_TYPE := TIME_INVALID;
signal long_latch, long_latch_next : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0');
signal payload_mem_full, payload_mem_full_next : std_logic := '0';
signal sample_mem_full, sample_mem_full_next : std_logic := '0';
@ -440,6 +439,7 @@ begin
unmark_instances_next <= unmark_instances;
is_first_instance_sample_next <= is_first_instance_sample;
dynamic_next_instance_next <= dynamic_next_instance;
last_read_ts_next <= last_read_ts;
ack_dds <= '0';
inst_data_variant <= '0';
done_dds <= '0';
@ -634,6 +634,22 @@ begin
-- Timestamp 2/2
when 2 =>
ts_latch_next(1) <= data_in_rtps;
-- Synthesis Guard
if (DESTINATION_ORDER_QOS = BY_SOURCE_TIMESTAMP_DESTINATION_ORDER_QOS) then
-- Check Timestamp
tmp_dw := (0 => ts_latch(0), 1 => unsigned(data_in_rtps));
-- NOTE: Equal timestamps are still acceptable, as they are inserted after the
-- highest last read sample.
-- Timestamp is smaller than highest last read (Or is Invalid)
if (tmp_dw /= TIME_INVALID or tmp_dw < last_read_ts) then
-- Drop Sample
stage_next <= SKIP_ADD_DROP;
end if;
end if;
-- Lifespan Deadline 2/2
when 4 =>
-- Skip Key Hash, if not available
@ -665,6 +681,7 @@ begin
sample_wen <= '0';
sample_addr_next <= sample_addr; -- Keep Addr (Payload Addr)
key_hash_next(3) <= data_in_rtps;
-- Writer Endpoint Position
when 9 =>
-- Latch Input, but do not pass to Memory
writer_pos_next <= to_integer(unsigned(data_in_rtps));
@ -678,7 +695,7 @@ begin
when ADD_PAYLOAD_ADDRESS =>
-- Precondition: sample_addr (Payload Address)
sample_addr_next <= sample_addr + 1; -- Instance Address
sample_addr_next <= cur_sample + SMF_INSTANCE_ADDR_OFFSET;
if (has_data = '1') then
-- Store Payload Address
@ -762,7 +779,7 @@ begin
-- No Empty Payload Slots available
if (payload_read_data = PAYLOAD_MEMORY_MAX_ADDRESS) then
-- Reject Change
stage_next <= SKIP_ADD;
stage_next <= SKIP_ADD_REJECT;
else
-- Latch next Payload Slot and Continue
cur_payload_next <= payload_read_data;
@ -808,7 +825,7 @@ begin
-- Wait for Instance Search to finish
if (inst_op_done = '1') then
sample_addr_next <= sample_addr + 1; -- Disposed Gen Counter
sample_addr_next <= cur_sample + SMF_DISPOSED_GEN_CNT_OFFSET;
-- Instance Found
if (inst_addr_base /= INSTANCE_MEMORY_MAX_ADDRESS) then
@ -819,25 +836,25 @@ begin
-- TIME_BASED_FILTER QOS
if (TIME_BASED_FILTER_QOS /= DURATION_ZERO and time <= inst_data.ignore_deadline) then
-- Drop Change
res_rtps <= ACCEPTED;
res_rtps <= ACCEPTED;
stage_next <= IDLE;
-- RESOURCE_LIMITS_QOS (MAX_SAMPLES_PER_INSTANCE)
elsif (MAX_SAMPLES_PER_INSTANCE /= LENGTH_UNLIMITED and inst_data.sample_cnt = MAX_SAMPLES_PER_INSTANCE) then
if (HISTORY_QOS = KEEP_LAST_HISTORY_QOS and RELIABILITY_QOS = RELIABLE_RELIABILITY_QOS) then
-- Reject Change
res_rtps <= REJECTED;
res_rtps <= REJECTED;
stage_next <= IDLE;
else
-- Accept Change (Remove Oldest Instance Sample)
remove_oldest_inst_sample_next <= '1';
res_rtps <= ACCEPTED;
res_rtps <= ACCEPTED;
stage_next <= UPDATE_INSTANCE;
end if;
-- RESOURCE_LIMITS_QOS (MAX_SAMPLES)
elsif (empty_sample_list_head = empty_sample_list_tail) then
if (HISTORY_QOS = KEEP_ALL_HISTORY_QOS and RELIABILITY_QOS = RELIABLE_RELIABILITY_QOS) then
-- Reject Change
res_rtps <= REJECTED;
res_rtps <= REJECTED;
stage_next <= IDLE;
else
-- Accept Change (Remove Oldest Sample)
@ -1050,7 +1067,7 @@ begin
case (cnt) is
-- Disposed Generation Counter
when 0 =>
sample_addr_next <= sample_addr + 1;
sample_addr_next <= cur_sample + SMF_NO_WRITERS_GEN_CNT_OFFSET;
sample_wen <= '1';
-- NOT_ALIVE_DISPOSED -> ALIVE Transition
@ -1074,60 +1091,76 @@ begin
if (newest_sample = SAMPLE_MEMORY_MAX_ADDRESS) then
stage_next <= FINALIZE_SAMPLE;
sample_addr_next <= empty_sample_list_head + SMF_PREV_ADDR_OFFSET;
cur_sample_next <= empty_sample_list_head;
next_sample_next <= SAMPLE_MEMORY_MAX_ADDRESS;
prev_sample_next <= SAMPLE_MEMORY_MAX_ADDRESS;
cnt_next <= 0;
elsif (DESTINATION_ORDER_QOS = BY_RECEPTION_TIMESTAMP_DESTINATION_ORDER_QOS) then
stage_next <= FIX_POINTERS;
prev_sample_next <= newest_sample;
sample_addr_next <= newest_sample + SMF_NEXT_ADDR_OFFSET;
cur_sample_next <= newest_sample;
cnt_next <= 0;
else
stage_next <= FIND_POS;
prev_sample_next <= newest_sample;
sample_addr_next <= newest_sample + SMF_TIMESTAMP_OFFSET;
cur_sample_next <= newest_sample;
cnt_next <= 0;
end if;
when others =>
null;
end case;
when FIND_POS =>
-- Precondition: prev_sample set, sample_addr (Timestamp 1/2 Addr of prev_sample)
-- Synthesis Guard
if (DESTINATION_ORDER_QOS /= BY_RECEPTION_TIMESTAMP_DESTINATION_ORDER_QOS) then
-- Precondition: prev_sample set, sample_addr (Timestamp 1/2 Addr of prev_sample)
cnt_next <= cnt + 1;
cnt_next <= cnt + 1;
case (cnt) is
-- Preload
when 0 =>
sample_addr_next <= sample_addr + 1;
sample_ren <= '1';
-- Timestamp 1/2
when 1 =>
sample_addr_next <= sample_addr + SMF_PREV_ADDR_OFFSET-(SMF_TIMESTAMP_OFFSET+1); -- Prev Addr
sample_ren <= '1';
long_latch_next <= sample_read_data;
-- Timestamp 2/2
when 2 =>
sample_ren <= '1';
case (cnt) is
-- Preload
when 0 =>
sample_addr_next <= sample_addr + 1; -- Timestamp 2/2
sample_ren <= '1';
-- Timestamp 1/2
when 1 =>
sample_addr_next <= cur_sample + SMF_PREV_ADDR_OFFSET;
sample_ren <= '1';
long_latch_next <= sample_read_data;
-- Timestamp 2/2
when 2 =>
sample_ren <= '1';
tmp_dw := (0 => unsigned(long_latch), 1 => unsigned(sample_read_data));
tmp_dw := (0 => unsigned(long_latch), 1 => unsigned(sample_read_data));
sample_addr_next <= sample_addr + SMF_NEXT_ADDR_OFFSET-SMF_PREV_ADDR_OFFSET; -- Next Addr
sample_addr_next <= cur_sample + SMF_NEXT_ADDR_OFFSET;
-- Found position (After current slot)
if (ts_latch >= tmp_dw) then
stage_next <= FIX_POINTERS;
cnt_next <= 0;
end if;
-- Previous Address
when 3 =>
-- No previous Slot (Oldest Sample)
if (sample_read_data = PAYLOAD_MEMORY_MAX_ADDRESS) then
assert (prev_sample = oldest_sample) report "Previous Sample is MAX_ADDR, but sample is not OLDEST (HEAD)" severity FAILURE;
-- Found position (After current slot)
if (ts_latch >= tmp_dw) then
stage_next <= FIX_POINTERS;
cnt_next <= 0;
end if;
-- Previous Pointer (Previous Sample)
when 3 =>
-- No previous Slot (Oldest Sample)
if (sample_read_data = PAYLOAD_MEMORY_MAX_ADDRESS) then
assert (prev_sample = oldest_sample) report "Previous Sample is MAX_ADDR, but sample is not OLDEST (HEAD)" severity FAILURE;
stage_next <= FIX_POINTERS;
cnt_next <= 0;
else
prev_sample_next <= sample_read_data;
sample_addr_next <= sample_read_data + SMF_TIMESTAMP_OFFSET;
cnt_next <= 0;
end if;
end case;
-- NOTE: Sample is added to HEAD of List
prev_sample_next <= SAMPLE_MEMORY_MAX_ADDRESS;
next_sample_next <= cur_sample;
stage_next <= FIX_POINTERS;
cnt_next <= 2; -- Skip to Previous Pointer Fix
else
-- Continue Search
prev_sample_next <= sample_read_data;
sample_addr_next <= sample_read_data + SMF_TIMESTAMP_OFFSET;
cnt_next <= 0;
end if;
end case;
end if;
when FIX_POINTERS =>
-- Precondition: sample_addr (Next Addr of prev_sample)
@ -1137,6 +1170,7 @@ begin
-- Preload
when 0 =>
sample_ren <= '1';
-- Next Pointer (Previous Sample)
when 1 =>
-- Fix Next Pointer
@ -1149,7 +1183,8 @@ begin
next_sample_next <= SAMPLE_MEMORY_MAX_ADDRESS;
stage_next <= FINALIZE_SAMPLE;
sample_addr_next <= empty_sample_list_head + SMF_PREV_ADDR_OFFSET; -- Prev Addr of New Sample
sample_addr_next <= empty_sample_list_head + SMF_PREV_ADDR_OFFSET;
cur_sample_next <= empty_sample_list_head;
cnt_next <= 0;
else
-- Latch Next Sample
@ -1157,13 +1192,15 @@ begin
sample_addr_next <= sample_read_data + SMF_PREV_ADDR_OFFSET; -- Prev Addr of Next Sample
end if;
-- Previous Pointer (Next Sample)
when 2 =>
-- Fix Previous Pointer
sample_write_data <= empty_sample_list_head;
sample_wen <= '1';
stage_next <= FINALIZE_SAMPLE;
sample_addr_next <= empty_sample_list_head + SMF_PREV_ADDR_OFFSET; -- Prev Addr of New Sample
sample_addr_next <= empty_sample_list_head + SMF_PREV_ADDR_OFFSET;
cur_sample_next <= empty_sample_list_head;
cnt_next <= 0;
when others =>
null;
@ -1176,7 +1213,7 @@ begin
case (cnt) is
-- Previous Sample Address
when 0 =>
sample_addr_next <= sample_addr + 1; -- Next Sample
sample_addr_next <= cur_sample + SMF_NEXT_ADDR_OFFSET;
-- Write Prev Addr
sample_write_data <= prev_sample;
@ -1439,7 +1476,7 @@ begin
stage_next <= IDLE;
end if;
end if;
when SKIP_ADD =>
when SKIP_ADD_REJECT =>
case (cnt) is
-- SKIP READ
when 0 =>
@ -1455,6 +1492,22 @@ begin
when others =>
null;
end case;
when SKIP_ADD_DROP =>
case (cnt) is
-- SKIP READ
when 0 =>
ready_in_rtps <= '1';
-- Wait until last word from input
if (last_word_in_rtps = '1') then
cnt_next <= 1;
end if;
-- DROP SAMPLE
when 1 =>
res_rtps <= ACCEPTED;
stage_next <= IDLE;
when others =>
null;
end case;
when REMOVE_WRITER =>
-- Memory Operation Guard
if (inst_op_done = '1') then
@ -2015,6 +2068,14 @@ begin
-- Invalidate Data
si_valid_sig_next <= '0';
-- Synthesis Guard
if (DESTINATION_ORDER_QOS = BY_SOURCE_TIMESTAMP_DESTINATION_ORDER_QOS) then
-- Update Last Read Timestamp
if (si_source_timestamp_sig > last_read_ts) then
last_read_ts_next <= si_source_timestamp_sig;
end if;
end if;
-- Sample not marked as Read
if (sample_status_info(READ_FLAG) /= '1') then
-- Mark Sample as Read