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

* Various Bug Fixes in rtps_handler

Browse Source 
* rtps_handler_test1 Complete and Passing
This commit is contained in:
Greek 2020-11-17 15:21:24 +01:00
parent 9c95e58e32
commit 9b4a2ed073
6 changed files with 471 additions and 364 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
sim/rtps_handler.do
View File

@ -14,21 +14,30 @@ add wave -noupdate /rtps_handler_test1/uut/builtin_wr
add wave -noupdate /rtps_handler_test1/uut/user_full
add wave -noupdate /rtps_handler_test1/uut/user_wr
add wave -noupdate /rtps_handler_test1/uut/last_word_out
add wave -noupdate -divider MISC
add wave -noupdate -divider TESTBENCH
add wave -noupdate /rtps_handler_test1/start
add wave -noupdate /rtps_handler_test1/stimulus.length
add wave -noupdate /rtps_handler_test1/stim_stage
add wave -noupdate /rtps_handler_test1/cnt_stim
add wave -noupdate /rtps_handler_test1/packet_sent
add wave -noupdate /rtps_handler_test1/reference.length
add wave -noupdate /rtps_handler_test1/ref_stage
add wave -noupdate /rtps_handler_test1/cnt_ref
add wave -noupdate /rtps_handler_test1/packet_checked
add wave -noupdate -divider RTPS_HANDLER
add wave -noupdate /rtps_handler_test1/uut/stage
add wave -noupdate /rtps_handler_test1/uut/stage_next
add wave -noupdate /rtps_handler_test1/uut/cnt
add wave -noupdate -radix unsigned /rtps_handler_test1/uut/read_cnt
add wave -noupdate -radix unsigned /rtps_handler_test1/uut/packet_length
add wave -noupdate -radix unsigned /rtps_handler_test1/uut/data_header_end
add wave -noupdate -radix unsigned /rtps_handler_test1/uut/sub_end
add wave -noupdate -divider MISC
TreeUpdate [SetDefaultTree]
WaveRestoreCursors {{Cursor 1} {0 ps} 0}
quietly wave cursor active 1
configure wave -namecolwidth 150
configure wave -valuecolwidth 63
WaveRestoreCursors {Begin {80925000 ps} 1} {Error {84575000 ps} 1} {{Cursor 3} {83975000 ps} 0}
quietly wave cursor active 3
configure wave -namecolwidth 132
configure wave -valuecolwidth 91
configure wave -justifyvalue left
configure wave -signalnamewidth 1
configure wave -snapdistance 10
@ -41,4 +50,4 @@ configure wave -griddelta 40
configure wave -timeline 0
configure wave -timelineunits ps
update
WaveRestoreZoom {0 ps} {1206528 ps}
WaveRestoreZoom {83443397 ps} {84642652 ps}

7
src/TODO.txt
View File

@ -78,7 +78,12 @@
- 9.4.5.1.2 Flags
Clarify from where the endianness begins.
One might think it would begin after the Submessage Header, but the length is also endian dependent.
- 9.4.5.3.1 Data Flags
"D=1 and K=1 is an invalid combination in this version of the protocol."
Does this invalidate the Submessage? Does 8.3.4.1 apply (Invalidate rest of Message)?
- 9.4.5.1.3 octetsToNextHeader
Similarly to "9.4.2.11" state that this is always a multiple of four.
* Source Port of SPDP is irrelevant, since it is BEST EFFORT and we do not reply (only Destination Port is of significance)

317
src/Tests/Level_0/rtps_handler_test1.vhd
View File

File diff suppressed because it is too large Load Diff

2
src/rtps_config_package.vhd
View File

@ -770,7 +770,7 @@ package body rtps_config_package is
variable ret : std_logic_vector(width-1 downto 0) := (others => '0');
begin
ret := slv(slv'length-1 downto slv'length-width);
if (slv(width-1 downto 0) /= (width-1 downto 0 => '0')) then
if (slv(slv'length-width-1 downto 0) /= (slv'length-width-1 downto 0 => '0')) then
ret := std_logic_vector(unsigned(ret) + 1);
end if;
return ret;

404
src/rtps_handler.vhd
View File

@ -49,10 +49,16 @@ architecture arch of rtps_handler is
signal reset_read_cnt : std_logic;
-- 4-Byte Word counter (Counts words read from input FIFO)
signal read_cnt : unsigned(UDP_HEADER_LENGTH_WIDTH-3 downto 0) := (others => '0');
-- read_cnt + 1
-- NOTE: Because the Submessage Length does not include the Submessage Header, we need a to add +1 to find the end of the Submessage
-- In order to prevent two serial additions in the same clock cycle, we use this pre-incremented signal instead
signal read_cnt_plus : unsigned(UDP_HEADER_LENGTH_WIDTH-3 downto 0) := (others => '0');
-- Total packet length (4-Byte Words)
signal packet_length, packet_length_next : unsigned(UDP_HEADER_LENGTH_WIDTH-3 downto 0) := (others => '0');
-- End of Submessage from the beginning of the UDP Packet in Bytes
signal sub_end, sub_end_next : unsigned(SUBMESSAGE_LENGTH_WIDTH-1 downto 0) := (others => '0');
-- End of Submessage from the beginning of the UDP Packet in 4-Byte Words
-- NOTE: We count this in 4-Byte Words, because Submessages always begin in a 4-Byte boundary (DDSI-RTPS 2.3 Section 9.4.1),
-- and thus the "sub_end" is always a multiple of four.
signal sub_end, sub_end_next : unsigned(SUBMESSAGE_LENGTH_WIDTH-3 downto 0) := (others => '0');
-- End of DATA Submessage Sub-Header (Beginning of inlineQoS/Payload) from the beginning of the UDP Packet in Bytes
signal data_header_end, data_header_end_next : unsigned(SUBMESSAGE_LENGTH_WIDTH-1 downto 0) := (others => '0');
-- Input Signal Latch. Used to read 4-Byte aligned from input (see align_prc)
@ -176,14 +182,14 @@ begin
begin
input := align_sig & data_in;
case(align_offset) is
when "00" =>
data_in_aligned <= input(31 downto 0);
when "01" =>
data_in_aligned <= input(39 downto 8);
data_in_aligned <= input(55 downto 24);
when "10" =>
data_in_aligned <= input(47 downto 16);
when others => --"11"
data_in_aligned <= input(55 downto 24);
when "11" =>
data_in_aligned <= input(39 downto 8);
when others => -- "00"
data_in_aligned <= input(31 downto 0);
end case;
end process;
@ -215,9 +221,11 @@ begin
-- SKIP_SUB Skip rest of Submessage
-- SKIP_PACKET Skip rest of UDP Packet
parse_prc: process(all)
variable tmp : std_logic_vector(0 to NUM_ENDPOINTS-1) := (others => '0');
variable dest : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0');
variable tmp_sn : SEQUENCENUMBER_TYPE := SEQUENCENUMBER_UNKNOWN;
variable tmp : std_logic_vector(0 to NUM_ENDPOINTS-1) := (others => '0');
variable dest : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0');
variable tmp_sn : SEQUENCENUMBER_TYPE := SEQUENCENUMBER_UNKNOWN;
-- This variable is used to allow the Overread Guard to control the rd Signal
variable rd_guard : std_logic := '0';
begin
--DEFAULT Registered
stage_next <= stage;
@ -254,6 +262,7 @@ begin
-- DEFAULT Unregistered
data_out <= (others => '0');
rd_sig <= '0';
rd_guard := '0';
reset_read_cnt <= '0';
wr_sig <= '0';
last_word_out <= '0';
@ -264,8 +273,8 @@ begin
when SRC_ADDR_HEADER =>
-- Input FIFO Guard
if (empty = '0') then
rd_sig <= '1';
reset_read_cnt <= '1';
rd_guard := '1';
reset_read_cnt <= '1';
src_addr_next <= data_in;
@ -275,7 +284,7 @@ begin
when DEST_ADDR_HEADER =>
-- Input FIFO Guard
if (empty = '0') then
rd_sig <= '1';
rd_guard := '1';
reset_read_cnt <= '1';
case (data_in) is
@ -293,13 +302,11 @@ begin
when LEN_HEADER =>
-- Input FIFO Guard
if (empty = '0') then
rd_sig <= '1';
rd_guard := '1';
-- NOTE: Read word counter starts counting the moment we leave this stage. (We can skip Packets from this stage on)
reset_read_cnt <= '1';
packet_length_next <= unsigned(data_in(packet_length'length-1 downto 0));
-- Valid Submessage End
sub_end_next <= unsigned(data_in(sub_end'length-1 downto 0));
-- DEFAULT
stage_next <= UDP_HEADER_1;
@ -308,7 +315,7 @@ begin
when UDP_HEADER_1 =>
-- Input FIFO Guard
if (empty = '0') then
rd_sig <= '1';
rd_guard := '1';
-- DEFAULT
is_metatraffic_next <= '0';
@ -356,7 +363,7 @@ begin
when UDP_HEADER_2 =>
-- Input FIFO Guard
if (empty = '0') then
rd_sig <= '1';
rd_guard := '1';
-- NOTE: The UDP checksum has to be validated before, because we passthrough the input to the output FIFOs before we reach the end of the UDP Packet.
-- SANITY CHECK: Check if UPD header length matches actual packet length
@ -371,7 +378,7 @@ begin
when RTPS_HEADER_1 =>
-- Input FIFO Guard
if (empty = '0') then
rd_sig <= '1';
rd_guard := '1';
-- If underlying Protocol is not RTPS, skip packet
if(data_in /= PROTOCOL_RTPS) then
@ -384,7 +391,7 @@ begin
when RTPS_HEADER_2 =>
-- Input FIFO Guard
if (empty = '0') then
rd_sig <= '1';
rd_guard := '1';
-- If RTPS Protocol Major Version is not 2, skip packet
if(rtps_version(15 downto 8) /= PROTOCOLVERSION_2_4(15 downto 8)) then
@ -398,7 +405,7 @@ begin
when RTPS_HEADER_3 =>
-- Input FIFO Guard
if (empty = '0') then
rd_sig <= '1';
rd_guard := '1';
cnt_next <= cnt + 1;
case (cnt) is
@ -421,7 +428,7 @@ begin
when RTPS_SUB_HEADER =>
-- Input FIFO Guard
if (empty = '0') then
rd_sig <= '1';
rd_guard := '1';
-- DEFAULT
src_is_reader_next <= '0';
@ -433,14 +440,13 @@ begin
-- If an unknown Submessage Type is encountered that can also have a valid zero Submessage length (Without being the last Submessage),
-- this will be seen here as "Last Submessage", and the complete rest of the Packet will be Skipped.
-- If Last Submessage, length is zero and actual size extend until end of packet
if (rtps_sub_length = 0) then
-- EXCEPTION: INFO_TS and PAD can have valid zero Submessage Length
if (rtps_sub_id /= SID_PAD and rtps_sub_id /= SID_INFO_TS) then
-- Fix Submessage End Position
sub_end_next <= packet_length & "00";
end if;
-- EXCEPTION: INFO_TS and PAD can have valid zero Submessage Length
if (rtps_sub_length = 0 and rtps_sub_id /= SID_PAD and rtps_sub_id /= SID_INFO_TS) then
-- Fix Submessage End Position
sub_end_next <= packet_length;
else
sub_end_next <= (read_cnt & "00") + rtps_sub_length;
-- NOTE: Submessage Length is always a multiple of four
sub_end_next <= read_cnt_plus + rtps_sub_length(rtps_sub_length'length-1 downto 2);
end if;
case (rtps_sub_id) is
@ -494,7 +500,8 @@ begin
-- VALIDITY CHECK
elsif (rtps_sub_flags(SUBMESSAGE_DATA_FLAG_POS) = '1' and rtps_sub_flags(SUBMESSAGE_KEY_FLAG_POS) = '1') then
-- Invalid Submessage, skip Packet (see DDSI-RTPS 2.3 Section 9.4.5.3.1 and 8.3.4.1)
stage_next <= SKIP_PACKET;
-- TODO: Clarify if this invalidate the rest of the Message, since it is not stated in 8.3.7.2.3
stage_next <= SKIP_SUB;
end if;
-- PAD (Variable Size Padding)
when SID_PAD =>
@ -507,7 +514,7 @@ begin
when PARSE_INFO_DST =>
-- Input FIFO Guard
if (empty = '0') then
rd_sig <= '1';
rd_guard := '1';
cnt_next <= cnt + 1;
-- If Destination GUID Prefix is not us, skip the rest of the packet
@ -539,7 +546,7 @@ begin
when PARSE_INFO_SRC =>
-- Input FIFO Guard
if (empty = '0') then
rd_sig <= '1';
rd_guard := '1';
cnt_next <= cnt + 1;
case (cnt) is
@ -549,7 +556,7 @@ begin
-- Protocol Version & Vendor ID
when 1 =>
-- Check Major Protocol Version
if (data_in_swapped(31 downto 24) /= PROTOCOLVERSION_2_4(15 downto 8)) then
if (data_in(31 downto 24) /= PROTOCOLVERSION_2_4(15 downto 8)) then
-- Protocol not supported, skip rest of Packet
stage_next <= SKIP_PACKET;
end if;
@ -568,7 +575,7 @@ begin
end case;
end if;
when PARSE_INFO_TS =>
rd_sig <= '1';
rd_guard := '1';
cnt_next <= cnt + 1;
case (cnt) is
@ -587,7 +594,7 @@ begin
when PARSE_INFO_REPLY =>
-- Input FIFO Guard
if (empty = '0') then
rd_sig <= '1';
rd_guard := '1';
-- Extract Locator List
numlocators_next <= unsigned(data_in_swapped(numlocators_next'length-1 downto 0));
@ -611,7 +618,7 @@ begin
else
-- Input FIFO Guard
if (empty = '0') then
rd_sig <= '1';
rd_guard := '1';
cnt_next <= cnt + 1;
case (cnt) is
@ -631,7 +638,7 @@ begin
if (data_in_swapped(UDP_PORT_INVALID'range) = UDP_PORT_INVALID) then
locator_match_next <= '0';
else
src_port_next <= data_in_swapped(src_port_next'length-1 downto 0);
long_latch_next <= data_in_swapped;
end if;
end if;
-- Locator Address 1/4
@ -646,8 +653,9 @@ begin
-- Locator Address 4/4 (IPv4 Address)
when 5 =>
-- We only store valid UDPv4 Locators
if (locator_match = '1' and data_in_swapped /= IPv4_ADDRESS_INVALID) then
src_addr_next <= data_in_swapped;
if (locator_match = '1' and data_in /= IPv4_ADDRESS_INVALID) then
src_addr_next <= data_in;
src_port_next <= long_latch(src_port_next'length-1 downto 0);
end if;
-- Last Word of Locator
numlocators_next <= numlocators - 1;
@ -660,7 +668,7 @@ begin
when PARSE_INFO_REPLY_IP4 =>
-- Input FIFO Guard
if (empty = '0') then
rd_sig <= '1';
rd_guard := '1';
cnt_next <= cnt + 1;
case (cnt) is
@ -671,18 +679,21 @@ begin
locator_match_next <= '0';
else
locator_match_next <= '1';
src_addr_next <= data_in_swapped;
long_latch_next <= data_in_swapped;
end if;
-- UDPv4 Port
when 1 =>
-- Store only valid Locators
if (locator_match = '1' and data_in_swapped(UDP_PORT_INVALID'range) /= UDP_PORT_INVALID) then
src_port_next <= data_in_swapped(src_port_next'length-1 downto 0);
src_addr_next <= long_latch;
end if;
-- Parse Multicast if available
if (flags(SUBMESSAGE_MULTICAST_FLAG_POS) = '1') then
locator_match_next <= '0';
-- Reset Flag to prevent loop
flags_next(SUBMESSAGE_MULTICAST_FLAG_POS) <= '0';
cnt_next <= 0;
else
-- DONE
@ -695,7 +706,7 @@ begin
when PARSE_HEARTBEAT =>
-- Input FIFO Guard
if (empty = '0') then
rd_sig <= '1';
rd_guard := '1';
cnt_next <= cnt + 1;
case (cnt) is
@ -752,7 +763,7 @@ begin
when PARSE_ACKNACK =>
-- Input FIFO Guard
if (empty = '0') then
rd_sig <= '1';
rd_guard := '1';
cnt_next <= cnt + 1;
case (cnt) is
@ -780,31 +791,29 @@ begin
-- ReaderSNState.NumBits
when 4 =>
ulong_latch_next <= unsigned(data_in_swapped);
-- XXX: Possible Worst Case Path (Comparison and two Arithmetic Operations in same Clock Cycle)
bitmap_cnt_next <= unsigned(round_slv(data_in_swapped(log2c(MAX_BITMAP_WIDTH)-1 downto 0),bitmap_cnt'length)) - 1;
bitmap_cnt_next <= unsigned(round_slv(data_in_swapped(log2c(MAX_BITMAP_WIDTH)-1 downto 0),bitmap_cnt'length));
cnt2_next <= 0;
-- VALIDITY CHECK
if (data_in_swapped(0) = '1' or unsigned(data_in_swapped) > 256) then
-- If numBits is negative or larger than 256, Number Set is invalid (see DDSI-RTPS 2.3 Section 9.4.2.6)
-- If readerSNState is invalid, skip Packet (see DDSI-RTPS 2.3 Section 8.3.7.1.3 and 8.3.4.1)
stage_next <= SKIP_PACKET;
-- Bitmap empty
elsif (unsigned(data_in_swapped) = 0) then
-- Skip to Count
cnt_next <= 6;
else
-- Parse Bitmap
cnt2_next <= 0;
end if;
-- ReaderSNState.Bitmap
when 5 =>
cnt2_next <= cnt2 + 1;
bitmap_latch_next(cnt2) <= data_in_swapped;
-- Keep Sub-State until whole Bitmap is read
if (cnt2 /= bitmap_cnt) then
-- Read Bitmap
if (cnt2 < bitmap_cnt) then
cnt2_next <= cnt2 + 1;
bitmap_latch_next(cnt2) <= data_in_swapped;
-- Keep Sub-State
cnt_next <= cnt;
-- Exit Condition
else
-- Prevent Input Latching
rd_guard := '0';
end if;
-- Count
when 6 =>
@ -819,7 +828,7 @@ begin
when PARSE_GAP =>
-- Input FIFO Guard
if (empty = '0') then
rd_sig <= '1';
rd_guard := '1';
cnt_next <= cnt + 1;
case (cnt) is
@ -834,7 +843,7 @@ begin
sn_latch_1_next(0) <= unsigned(data_in_swapped);
-- GapStart Sequence Number 2/2
when 3 =>
sn_latch_1_next(0) <= unsigned(data_in_swapped);
sn_latch_1_next(1) <= unsigned(data_in_swapped);
-- VALIDITY CHECK
tmp_sn := (0 => sn_latch_1(0), 1 => unsigned(data_in_swapped));
@ -860,32 +869,29 @@ begin
-- ReaderSNState.NumBits
when 6 =>
ulong_latch_next <= unsigned(data_in_swapped);
-- XXX: Possible Worst Case Path (Comparison and two Arithmetic Operations in same Clock Cycle)
bitmap_cnt_next <= unsigned(round_slv(data_in_swapped(log2c(MAX_BITMAP_WIDTH)-1 downto 0),bitmap_cnt'length)) - 1;
bitmap_cnt_next <= unsigned(round_slv(data_in_swapped(log2c(MAX_BITMAP_WIDTH)-1 downto 0),bitmap_cnt'length));
cnt2_next <= 0;
-- VALIDITY CHECK
if (data_in_swapped(0) = '1' or unsigned(data_in_swapped) > 256) then
-- If numBits is negative or larger than 256, Number Set is invalid (see DDSI-RTPS 2.3 Section 9.4.2.6)
-- If gapList is invalid, skip Packet (see DDSI-RTPS 2.3 Section 8.3.7.4.3 and 8.3.4.1)
stage_next <= SKIP_PACKET;
-- Bitmap empty
elsif(unsigned(data_in_swapped) = 0) then
-- DONE
stage_next <= MATCH_DST_ENDPOINT;
else
-- Parse Bitmap
cnt2_next <= 0;
end if;
-- ReaderSNState.Bitmap
when 7 =>
cnt2_next <= cnt2 + 1;
bitmap_latch_next(cnt2) <= data_in_swapped;
-- Keep Sub-State until whole Bitmap is read
if (cnt2 /= bitmap_cnt) then
-- Read Bitmap
if (cnt2 < bitmap_cnt) then
cnt2_next <= cnt2 + 1;
bitmap_latch_next(cnt2) <= data_in_swapped;
-- Keep Sub-State
cnt_next <= cnt;
-- Exit Condition
else
-- Prevent Input Latching
rd_guard := '0';
-- DONE
stage_next <= MATCH_DST_ENDPOINT;
end if;
@ -896,7 +902,7 @@ begin
when PARSE_DATA =>
-- Input FIFO Guard
if (empty = '0') then
rd_sig <= '1';
rd_guard := '1';
cnt_next <= cnt + 1;
case (cnt) is
@ -905,7 +911,7 @@ begin
-- NOTE: Extra Flags are unused
-- Latch Length to skip Uknown Data Header Part and latch offset to ensure 4-Byte alignement (see align_prc)
offset_latch_next <= std_logic_vector(rtps_sub_data_length(1 downto 0));
data_header_end_next <= (read_cnt & "00") + rtps_sub_data_length;
data_header_end_next <= (read_cnt_plus & "00") + rtps_sub_data_length;
-- Reader Entity ID
when 1 =>
dest_entityid_next <= data_in;
@ -933,77 +939,71 @@ begin
end case;
end if;
when SKIP_DATA_HEADER =>
if (empty = '0') then
-- End of Data Header
if (read_cnt >= data_header_end) then
stage_next <= MATCH_DST_ENDPOINT;
cnt_next <= 0;
-- Fix alignement
align_offset_next <= offset_latch;
-- Latch Input for alignment purposes
align_sig_next <= data_in(23 downto 0);
else
-- Skip-Read
rd_sig <= '1';
end if;
-- End of Data Header
if ((read_cnt & "00") >= data_header_end) then
stage_next <= MATCH_DST_ENDPOINT;
cnt_next <= 0;
-- Fix alignement
align_offset_next <= offset_latch;
-- Input Guard
elsif(empty = '0') then
-- Latch Input for alignment purposes
align_sig_next <= data_in(23 downto 0);
-- Skip-Read
rd_guard := '1';
end if;
when MATCH_DST_ENDPOINT =>
-- Input FIFO Guard
if (empty = '0') then
rd_sig <= '1';
-- DEFAULT
user_endpoint_next <= (others => '0');
builtin_endpoint_next <= '0';
stage_next <= PUSH_PAYLOAD_HEADER;
cnt_next <= 0;
-- *Check Dest Entity ID*
-- Target ALL Endpoints
if (dest_entityid = ENTITYID_UNKNOWN) then
if (is_metatraffic = '1') then
builtin_endpoint_next <= '1';
else
-- Mark Only Writers
if (src_is_reader = '1' and NUM_WRITERS /= 0) then
user_endpoint_next <= not ENDPOINT_READERS;
-- Mark Only Readers
elsif (NUM_READERS /= 0) then
user_endpoint_next <= ENDPOINT_READERS;
end if;
end if;
-- Target Built-In Endpoints
elsif (is_metatraffic = '1' and dest_entityid(7 downto 6) = BUILT_IN_ENTITY) then
-- DEFAULT
user_endpoint_next <= (others => '0');
builtin_endpoint_next <= '0';
stage_next <= PUSH_PAYLOAD_HEADER;
cnt_next <= 0;
-- *Check Dest Entity ID*
-- Target ALL Endpoints
if (dest_entityid = ENTITYID_UNKNOWN) then
if (is_metatraffic = '1') then
builtin_endpoint_next <= '1';
-- Match User Entity ID
elsif (is_metatraffic = '0') then
tmp := (others => '0');
for i in 0 to ENTITYID'length-1 loop
if (dest_entityid = ENTITYID(i)) then
tmp(i) := '1';
end if;
end loop;
-- Entity non existent, skip Submessage
if (tmp = (tmp'range => '0')) then
stage_next <= SKIP_SUB;
-- Entity ID Match
else
-- SANITY CHECK: Allow only Reader-Writer Communication
if (src_is_reader = '1') then
-- Mark only Writers
user_endpoint_next <= tmp and (not ENDPOINT_READERS);
else
-- Mark only Readers
user_endpoint_next <= tmp and ENDPOINT_READERS;
end if;
end if;
-- Destination Unreachable, skip Submessage
else
stage_next <= SKIP_SUB;
-- Mark Only Writers
if (src_is_reader = '1' and NUM_WRITERS /= 0) then
user_endpoint_next <= not ENDPOINT_READERS;
-- Mark Only Readers
elsif (NUM_READERS /= 0) then
user_endpoint_next <= ENDPOINT_READERS;
end if;
end if;
-- Target Built-In Endpoints
elsif (is_metatraffic = '1' and dest_entityid(7 downto 6) = BUILT_IN_ENTITY) then
builtin_endpoint_next <= '1';
-- Match User Entity ID
elsif (is_metatraffic = '0') then
tmp := (others => '0');
for i in 0 to ENTITYID'length-1 loop
if (dest_entityid = ENTITYID(i)) then
tmp(i) := '1';
end if;
end loop;
-- Entity non existent, skip Submessage
if (tmp = (tmp'range => '0')) then
stage_next <= SKIP_SUB;
-- Entity ID Match
else
-- SANITY CHECK: Allow only Reader-Writer Communication
if (src_is_reader = '1') then
-- Mark only Writers
user_endpoint_next <= tmp and (not ENDPOINT_READERS);
else
-- Mark only Readers
user_endpoint_next <= tmp and ENDPOINT_READERS;
end if;
end if;
-- Destination Unreachable, skip Submessage
else
stage_next <= SKIP_SUB;
end if;
when PUSH_PAYLOAD_HEADER =>
-- NOTE: This is a synchronised push on potentially multiple output FIFOs. If one FIFO gets full, the process stalls for all FIFOs.
@ -1126,14 +1126,16 @@ begin
cnt2_next <= 0;
-- ReaderSNState.Bitmap
when 3 =>
cnt2_next <= cnt2 + 1;
data_out <= bitmap_latch_next(cnt2);
wr_sig <= '1';
-- Keep Sub-State until Bitmap is written
if (cnt2 /= bitmap_cnt) then
-- Write Bitmap
if (cnt2 < bitmap_cnt) then
cnt2_next <= cnt2 + 1;
data_out <= bitmap_latch_next(cnt2);
wr_sig <= '1';
-- Keep Sub-State
cnt_next <= cnt;
-- Exit Condition
end if;
-- Count
when 4 =>
@ -1171,14 +1173,16 @@ begin
cnt2_next <= 0;
-- GapList.Bitmap
when 5 =>
cnt2_next <= cnt2 + 1;
data_out <= bitmap_latch_next(cnt2);
wr_sig <= '1';
-- Keep Sub-State until Bitmap is written
if (cnt2 /= bitmap_cnt) then
-- Write Bitmap
if (cnt2 < bitmap_cnt) then
cnt2_next <= cnt2 + 1;
data_out <= bitmap_latch_next(cnt2);
wr_sig <= '1';
-- Keep Sub-State
cnt_next <= cnt;
-- Exit Condition
else
-- DONE
stage_next <= SKIP_SUB;
@ -1187,9 +1191,18 @@ begin
null;
end case;
when SID_DATA =>
-- Last Payload Word
if (read_cnt = sub_end) then
last_word_out <= '1';
-- Begin parsing of next submessage
stage_next <= RTPS_SUB_HEADER;
-- Reset alignement
align_offset_next <= (others => '0');
-- Reset Submessage End
sub_end_next <= (others => '1');
-- Input FIFO Guard
if (empty = '0') then
rd_sig <= '1';
elsif (empty = '0') then
rd_guard := '1';
-- Latch Input for alignment purposes
align_sig_next <= data_in(23 downto 0);
@ -1197,15 +1210,6 @@ begin
-- Push Payload
data_out <= data_in_aligned;
wr_sig <= '1';
-- Last Payload Word
if (read_cnt & "00" >= sub_end) then
last_word_out <= '1';
-- Begin parsing of next submessage
stage_next <= RTPS_SUB_HEADER;
-- Reset alignement
align_offset_next <= (others => '0');
end if;
end if;
when others =>
stage_next <= SKIP_SUB;
@ -1213,24 +1217,30 @@ begin
end if;
when SKIP_SUB =>
-- End of Submessage
if ((read_cnt & "00") >= sub_end) then
if (read_cnt = sub_end) then
-- Begin parsing of next submessage
stage_next <= RTPS_SUB_HEADER;
-- Valid Submessage End
sub_end_next <= packet_length & "00";
-- Reset Submessage End
sub_end_next <= (others => '1');
-- Input FIFO Guard
elsif (empty = '0') then
-- Skip-Read
rd_sig <= '1';
rd_guard := '1';
end if;
when SKIP_PACKET =>
-- Valid Submessage End
sub_end_next <= packet_length & "00";
-- Reset Submessage End
sub_end_next <= (others => '1');
-- End of Packet
if (read_cnt = packet_length) then
-- Continue parsing next Packet
stage_next <= SRC_ADDR_HEADER;
-- Reset Packet Length
packet_length_next <= (others => '1');
-- Input FIFO Guard
if (empty = '0') then
elsif (empty = '0') then
-- Skip-Read
rd_sig <= '1';
rd_guard := '1';
end if;
-- NOTE: Exit condition is via the OVERREAD GUARD
when others =>
@ -1238,29 +1248,37 @@ begin
end case;
-- OVERREAD GUARD
-- Reached End of Packet
if (read_cnt = packet_length) then
-- XXX: Read from signal we set
-- Read outside of packet Length
-- NOTE: If the Packet Length is smaller than expected there will be a read from input FIFO while
-- the Packet Length has been reached and will be caught by this clause.
-- The SKIP_PACKET clause prevents a read signal from occuring in this situation, and thus prevents from entering this state.
if (read_cnt = packet_length and rd_guard = '1') then
-- Force rd_sig low
rd_sig <= '0';
-- NOTE: This clause is both "legally" taken (e.g. through SKIP_PACKET), or also "illegally" (unexpected) during read sequences (If Packet has less size than expected)
-- Because this clause can also be taken while we are pushing to the Endpoints, we inform possible listening Endpoints of the abrubt ending.
-- Endpoints thus can receive incomplete DATA PAYLOADs that they have to handle accordingly.
rd_sig <= '0';
-- Notify Endpoints of EOP
last_word_out <= '1';
-- Continue parsing next
-- Continue parsing next Packet
stage_next <= SRC_ADDR_HEADER;
-- Reset packet_length until new packet_length is read (Prevent false positives)
-- Reset Lengths
packet_length_next <= (others => '1');
sub_end_next <= (others => '1');
-- Read outside of Submessage Length
-- XXX: Read from signal we set
-- NOTE: If the Submessage Length is smaller than expected for a particular Submessage, there will be a read from input FIFO while
-- the Submessage Length has been reached and will be caught by this clause.
-- The SKIP_SUB clause prevents a read signal from occuring in this situation, and thus prevents from entering this state.
elsif (rd_sig = '1' and (read_cnt & "00") >= sub_end) then
elsif (read_cnt = sub_end and rd_guard = '1') then
-- Force rd_sig low
rd_sig <= '0';
-- Notify Endpoints of EOS
last_word_out <= '1';
-- Invalid Submessage Length Field, Skip Packet (see DDSI-RTPS 2.3 Section 8.3.4.1)
stage_next <= SKIP_PACKET;
-- Valid Submessage End
sub_end_next <= packet_length & "00";
-- Reset Submessage End
sub_end_next <= (others => '1');
-- DEFAULT
else
rd_sig <= rd_guard;
end if;
end process;
@ -1271,10 +1289,12 @@ begin
if rising_edge(clk) then
-- Reset Read counter
if (reset = '1' or reset_read_cnt = '1') then
read_cnt <= (others => '0');
read_cnt <= (others => '0');
read_cnt_plus <= to_unsigned(1, read_cnt_plus'length);
-- Increment read counter each time rd is high
elsif (rd_sig = '1') then
read_cnt <= read_cnt + 1;
read_cnt <= read_cnt + 1;
read_cnt_plus <= read_cnt_plus + 1;
end if;
end if;
end process;

84
src/rtps_test_package.vhd
View File

@ -1,8 +1,9 @@
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use ieee.math_real.UNIFORM;
use ieee.math_real.ROUND;
library osvvm; -- Utility Library
context osvvm.OsvvmContext;
use work.math_pkg.all;
use work.rtps_package.all;
@ -192,9 +193,7 @@ package rtps_test_package is
procedure gen_endpoint_data( ref : in ENDPOINT_DATA_TYPE; output : inout TEST_PACKET_TYPE);
impure function rand_int(min_val, max_val : integer) return integer;
impure function rand_slv(len : integer) return std_logic_vector;
impure function gen_rand_loc return LOCATOR_TYPE;
procedure gen_rand_loc(RV : inout RandomPType; ret : out LOCATOR_TYPE);
function int(n : integer; width : natural) return std_logic_vector;
end package;
@ -202,34 +201,13 @@ end package;
package body rtps_test_package is
-- *UTILITY FUNCTIONS*
impure function rand_int(min_val, max_val : integer) return integer is
variable r : real;
variable SEED : natural := 999;
begin
UNIFORM(SEED, SEED, r);
return integer(ROUND(r * real(max_val - min_val + 1) + real(min_val) - 0.5));
end function;
impure function rand_slv(len : integer) return std_logic_vector is
variable r : real;
variable slv : std_logic_vector(len - 1 downto 0);
variable SEED : natural := 999;
begin
for i in slv'range loop
uniform(SEED, SEED, r);
slv(i) := '1' when r > 0.5 else '0';
end loop;
return slv;
end function;
impure function gen_rand_loc return LOCATOR_TYPE is
variable ret : LOCATOR_TYPE := EMPTY_LOCATOR;
procedure gen_rand_loc(RV : inout RandomPType; ret : out LOCATOR_TYPE) is
begin
ret := EMPTY_LOCATOR;
ret.kind := LOCATOR_KIND_UDPv4;
ret.portn(UDP_PORT_WIDTH-1 downto 0) := rand_slv(UDP_PORT_WIDTH);
ret.addr(IPv4_ADDRESS_WIDTH-1 downto 0) := rand_slv(IPv4_ADDRESS_WIDTH);
return ret;
end function;
ret.portn(UDP_PORT_WIDTH-1 downto 0) := RV.RandSlv(UDP_PORT_WIDTH);
ret.addr(IPv4_ADDRESS_WIDTH-1 downto 0) := RV.RandSlv(IPv4_ADDRESS_WIDTH);
end procedure;
function int(n : integer; width : natural) return std_logic_vector is
begin
@ -238,9 +216,9 @@ package body rtps_test_package is
-- *DEFERRED DEFINITIONS*
constant DEFAULT_GUIDPREFIX : GUIDPREFIX_TYPE := (0 => rand_slv(WORD_WIDTH), 1 => rand_slv(WORD_WIDTH), 2 => rand_slv(WORD_WIDTH));
constant DEFAULT_GUIDPREFIX : GUIDPREFIX_TYPE := (0 => x"da27cc3c", 1 => x"687ddcde", 2 => x"88bce3d1");
constant DEFAULT_ENTITYID : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := rand_slv(ENTITYID_WIDTH);
constant DEFAULT_ENTITYID : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := x"b9cbad8d";
constant EMPTY_LOCATOR : LOCATOR_TYPE := (
kind => LOCATOR_KIND_INVALID,
@ -420,6 +398,15 @@ package body rtps_test_package is
procedure gen_udp_header(ref : in UDP_HEADER_TYPE; output : inout TEST_PACKET_TYPE) is
begin
-- IPv4 Source Address
output.data(output.length) := ref.src.addr(IPv4_ADDRESS_WIDTH-1 downto 0);
output.length := output.length + 1;
-- IPv4 Destination Address
output.data(output.length) := ref.dest.addr(IPv4_ADDRESS_WIDTH-1 downto 0);
output.length := output.length + 1;
-- Packet Length
output.data(output.length) := (others => '0');
output.length := output.length + 1;
-- Source Port & Destination Port
output.data(output.length) := ref.src.portn(UDP_PORT_WIDTH-1 downto 0) & ref.dest.portn(UDP_PORT_WIDTH-1 downto 0);
output.length := output.length + 1;
@ -431,7 +418,10 @@ package body rtps_test_package is
procedure fix_udp_packet(output : inout TEST_PACKET_TYPE) is
begin
assert (output.length < 2**UDP_PORT_WIDTH) report "Exceeded maximum UDP Packet Size" severity error;
output.data(1)(31 downto 16) := int(output.length*4, UDP_HEADER_LENGTH_WIDTH);
-- Fix Packet Length
output.data(2) := int(output.length-3, WORD_WIDTH);
-- Fix UDP Length
output.data(4)(31 downto 16) := int((output.length-3)*4, UDP_HEADER_LENGTH_WIDTH);
-- TODO: Calculate Checksum
end procedure;
@ -460,7 +450,7 @@ package body rtps_test_package is
output.data(output.length) := endian_swap(littleEndian, std_logic_vector(input.base(1)));
output.length := output.length + 1;
-- Sequence Number State (NumBits)
assert (unsigned(input.numBits) > 256) report "SequenceNS.numbits is higher than 256." severity warning;
assert (unsigned(input.numBits) <= 256) report "SequenceNS.numbits is higher than 256." severity warning;
output.data(output.length) := endian_swap(littleEndian, input.numBits);
output.length := output.length + 1;
-- Sequence Number State (Bitmap)
@ -478,7 +468,7 @@ package body rtps_test_package is
output.data(output.length) := endian_swap(littleEndian, input.base);
output.length := output.length + 1;
-- Sequence Number State (NumBits)
assert (unsigned(input.numBits) > 256) report "SequenceNS.numbits is higher than 256." severity warning;
assert (unsigned(input.numBits) <= 256) report "SequenceNS.numbits is higher than 256." severity warning;
output.data(output.length) := endian_swap(littleEndian, input.numBits);
output.length := output.length + 1;
-- Sequence Number State (Bitmap)
@ -612,7 +602,7 @@ package body rtps_test_package is
tmp := to_integer(unsigned(ref.octetsToInlineQos)) - 16;
if (tmp > 0) then
for i in 0 to tmp-1 loop
store_byte(i mod 4, rand_slv(WORD_WIDTH), i mod 4, output);
store_byte((i+1) mod 4, x"DEADBEEF", (i+1) mod 4, output);
if (i mod 4 = 3) then
output.length := output.length + 1;
output.data(output.length) := (others => '0');
@ -630,7 +620,7 @@ package body rtps_test_package is
end loop;
end if;
-- Fix Alignement
if ((tmp + ref.data.length*4) mod 4 /= 0) then
if ((tmp + (ref.data.length*4)) mod 4 /= 0) then
output.length := output.length + 1;
end if;
end if;
@ -652,17 +642,17 @@ package body rtps_test_package is
-- *INFO_REPLY_IP4*
elsif (ref.submessageID = SID_INFO_REPLY_IP4) then
-- Unicast Locator (Address)
output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), ref.unicastLocator.addr(IPv4_ADDRESS_WIDTH-1 downto 0));
output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), ref.unicastLocator.addr(CDR_LONG_WIDTH-1 downto 0));
output.length := output.length + 1;
-- Unicast Locator (Port)
output.data(output.length)(UDP_PORT_WIDTH-1 downto 0) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), ref.unicastLocator.portn(UDP_PORT_WIDTH-1 downto 0));
output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), ref.unicastLocator.portn);
output.length := output.length + 1;
if (ref.flags(SUBMESSAGE_MULTICAST_FLAG_POS) = '1') then
-- Multicast Locator (Address)
output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), ref.multicastLocator.addr(IPv4_ADDRESS_WIDTH-1 downto 0));
output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), ref.multicastLocator.addr(CDR_LONG_WIDTH-1 downto 0));
output.length := output.length + 1;
-- Multicast Locator (Port)
output.data(output.length)(UDP_PORT_WIDTH-1 downto 0) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), ref.multicastLocator.portn(UDP_PORT_WIDTH-1 downto 0));
output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), ref.multicastLocator.portn);
output.length := output.length + 1;
end if;
-- *INFO_TS*
@ -691,7 +681,7 @@ package body rtps_test_package is
if (ref.submessageID = SID_ACKNACK or ref.submessageID = SID_NACK_FRAG or ref.submessageID = SID_HEARTBEAT or ref.submessageID = SID_HEARTBEAT_FRAG or ref.submessageID = SID_GAP or ref.submessageID = SID_DATA or ref.submessageID = SID_DATA_FRAG or ref.submessageID = SID_INFO_TS or ref.submessageID = SID_INFO_SRC or ref.submessageID = SID_INFO_DST or ref.submessageID = SID_INFO_REPLY or ref.submessageID =SID_INFO_REPLY_IP4) then
-- Fix Submessage Length
if (ref.submessageLength = (ref.submessageLength'range => '1')) then
output.data(start)(SUBMESSAGE_LENGTH_WIDTH-1 downto 0) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), int(output.length-start-1,SUBMESSAGE_LENGTH_WIDTH));
output.data(start)(SUBMESSAGE_LENGTH_WIDTH-1 downto 0) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), int((output.length-start-1)*4,SUBMESSAGE_LENGTH_WIDTH));
-- Fix Packet Length
else
output.length := start + to_integer(unsigned(ref.submessageLength));
@ -699,7 +689,9 @@ package body rtps_test_package is
-- *PAD/UKNOWN*
else
-- Padding
for i in 0 to to_integer(unsigned(ref.submessageLength))-1 loop
tmp := to_integer(unsigned(ref.submessageLength));
assert (tmp mod 4 = 0) report "Padding Length has to be multiple of four" severity FAILURE;
for i in 0 to (tmp/4)-1 loop
output.data(output.length) := (others => '0');
output.length := output.length + 1;
end loop;
@ -749,9 +741,9 @@ package body rtps_test_package is
output.length := output.length + 1;
-- Timestamp
if (not is_meta) then
output.data(output.length) := std_logic_vector(ref.timestamp(0));
output.data(output.length) := std_logic_vector(ts(0));
output.length := output.length + 1;
output.data(output.length) := std_logic_vector(ref.timestamp(1));
output.data(output.length) := std_logic_vector(ts(1));
output.length := output.length + 1;
end if;
-- DATA PAYLOAD