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_full
add wave -noupdate /rtps_handler_test1/uut/user_wr add wave -noupdate /rtps_handler_test1/uut/user_wr
add wave -noupdate /rtps_handler_test1/uut/last_word_out 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/stimulus.length
add wave -noupdate /rtps_handler_test1/stim_stage
add wave -noupdate /rtps_handler_test1/cnt_stim add wave -noupdate /rtps_handler_test1/cnt_stim
add wave -noupdate /rtps_handler_test1/packet_sent add wave -noupdate /rtps_handler_test1/packet_sent
add wave -noupdate /rtps_handler_test1/reference.length 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/cnt_ref
add wave -noupdate /rtps_handler_test1/packet_checked add wave -noupdate /rtps_handler_test1/packet_checked
add wave -noupdate -divider RTPS_HANDLER add wave -noupdate -divider RTPS_HANDLER
add wave -noupdate /rtps_handler_test1/uut/stage add wave -noupdate /rtps_handler_test1/uut/stage
add wave -noupdate /rtps_handler_test1/uut/stage_next 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] TreeUpdate [SetDefaultTree]
WaveRestoreCursors {{Cursor 1} {0 ps} 0} WaveRestoreCursors {Begin {80925000 ps} 1} {Error {84575000 ps} 1} {{Cursor 3} {83975000 ps} 0}
quietly wave cursor active 1 quietly wave cursor active 3
configure wave -namecolwidth 150 configure wave -namecolwidth 132
configure wave -valuecolwidth 63 configure wave -valuecolwidth 91
configure wave -justifyvalue left configure wave -justifyvalue left
configure wave -signalnamewidth 1 configure wave -signalnamewidth 1
configure wave -snapdistance 10 configure wave -snapdistance 10
@ -41,4 +50,4 @@ configure wave -griddelta 40
configure wave -timeline 0 configure wave -timeline 0
configure wave -timelineunits ps configure wave -timelineunits ps
update 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 - 9.4.5.1.2 Flags
Clarify from where the endianness begins. Clarify from where the endianness begins.
One might think it would begin after the Submessage Header, but the length is also endian dependent. 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) * 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'); variable ret : std_logic_vector(width-1 downto 0) := (others => '0');
begin begin
ret := slv(slv'length-1 downto slv'length-width); 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); ret := std_logic_vector(unsigned(ret) + 1);
end if; end if;
return ret; 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; signal reset_read_cnt : std_logic;
-- 4-Byte Word counter (Counts words read from input FIFO) -- 4-Byte Word counter (Counts words read from input FIFO)
signal read_cnt : unsigned(UDP_HEADER_LENGTH_WIDTH-3 downto 0) := (others => '0'); 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) -- Total packet length (4-Byte Words)
signal packet_length, packet_length_next : unsigned(UDP_HEADER_LENGTH_WIDTH-3 downto 0) := (others => '0'); 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 -- End of Submessage from the beginning of the UDP Packet in 4-Byte Words
signal sub_end, sub_end_next : unsigned(SUBMESSAGE_LENGTH_WIDTH-1 downto 0) := (others => '0'); -- 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 -- 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'); 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) -- Input Signal Latch. Used to read 4-Byte aligned from input (see align_prc)
@ -176,14 +182,14 @@ begin
begin begin
input := align_sig & data_in; input := align_sig & data_in;
case(align_offset) is case(align_offset) is
when "00" =>
data_in_aligned <= input(31 downto 0);
when "01" => when "01" =>
data_in_aligned <= input(39 downto 8); data_in_aligned <= input(55 downto 24);
when "10" => when "10" =>
data_in_aligned <= input(47 downto 16); data_in_aligned <= input(47 downto 16);
when others => --"11" when "11" =>
data_in_aligned <= input(55 downto 24); data_in_aligned <= input(39 downto 8);
when others => -- "00"
data_in_aligned <= input(31 downto 0);
end case; end case;
end process; end process;
@ -215,9 +221,11 @@ begin
-- SKIP_SUB Skip rest of Submessage -- SKIP_SUB Skip rest of Submessage
-- SKIP_PACKET Skip rest of UDP Packet -- SKIP_PACKET Skip rest of UDP Packet
parse_prc: process(all) parse_prc: process(all)
variable tmp : std_logic_vector(0 to NUM_ENDPOINTS-1) := (others => '0'); 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 dest : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0');
variable tmp_sn : SEQUENCENUMBER_TYPE := SEQUENCENUMBER_UNKNOWN; 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 begin
--DEFAULT Registered --DEFAULT Registered
stage_next <= stage; stage_next <= stage;
@ -254,6 +262,7 @@ begin
-- DEFAULT Unregistered -- DEFAULT Unregistered
data_out <= (others => '0'); data_out <= (others => '0');
rd_sig <= '0'; rd_sig <= '0';
rd_guard := '0';
reset_read_cnt <= '0'; reset_read_cnt <= '0';
wr_sig <= '0'; wr_sig <= '0';
last_word_out <= '0'; last_word_out <= '0';
@ -264,8 +273,8 @@ begin
when SRC_ADDR_HEADER => when SRC_ADDR_HEADER =>
-- Input FIFO Guard -- Input FIFO Guard
if (empty = '0') then if (empty = '0') then
rd_sig <= '1'; rd_guard := '1';
reset_read_cnt <= '1'; reset_read_cnt <= '1';
src_addr_next <= data_in; src_addr_next <= data_in;
@ -275,7 +284,7 @@ begin
when DEST_ADDR_HEADER => when DEST_ADDR_HEADER =>
-- Input FIFO Guard -- Input FIFO Guard
if (empty = '0') then if (empty = '0') then
rd_sig <= '1'; rd_guard := '1';
reset_read_cnt <= '1'; reset_read_cnt <= '1';
case (data_in) is case (data_in) is
@ -293,13 +302,11 @@ begin
when LEN_HEADER => when LEN_HEADER =>
-- Input FIFO Guard -- Input FIFO Guard
if (empty = '0') then 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) -- NOTE: Read word counter starts counting the moment we leave this stage. (We can skip Packets from this stage on)
reset_read_cnt <= '1'; reset_read_cnt <= '1';
packet_length_next <= unsigned(data_in(packet_length'length-1 downto 0)); 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 -- DEFAULT
stage_next <= UDP_HEADER_1; stage_next <= UDP_HEADER_1;
@ -308,7 +315,7 @@ begin
when UDP_HEADER_1 => when UDP_HEADER_1 =>
-- Input FIFO Guard -- Input FIFO Guard
if (empty = '0') then if (empty = '0') then
rd_sig <= '1'; rd_guard := '1';
-- DEFAULT -- DEFAULT
is_metatraffic_next <= '0'; is_metatraffic_next <= '0';
@ -356,7 +363,7 @@ begin
when UDP_HEADER_2 => when UDP_HEADER_2 =>
-- Input FIFO Guard -- Input FIFO Guard
if (empty = '0') then 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. -- 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 -- SANITY CHECK: Check if UPD header length matches actual packet length
@ -371,7 +378,7 @@ begin
when RTPS_HEADER_1 => when RTPS_HEADER_1 =>
-- Input FIFO Guard -- Input FIFO Guard
if (empty = '0') then if (empty = '0') then
rd_sig <= '1'; rd_guard := '1';
-- If underlying Protocol is not RTPS, skip packet -- If underlying Protocol is not RTPS, skip packet
if(data_in /= PROTOCOL_RTPS) then if(data_in /= PROTOCOL_RTPS) then
@ -384,7 +391,7 @@ begin
when RTPS_HEADER_2 => when RTPS_HEADER_2 =>
-- Input FIFO Guard -- Input FIFO Guard
if (empty = '0') then if (empty = '0') then
rd_sig <= '1'; rd_guard := '1';
-- If RTPS Protocol Major Version is not 2, skip packet -- If RTPS Protocol Major Version is not 2, skip packet
if(rtps_version(15 downto 8) /= PROTOCOLVERSION_2_4(15 downto 8)) then if(rtps_version(15 downto 8) /= PROTOCOLVERSION_2_4(15 downto 8)) then
@ -398,7 +405,7 @@ begin
when RTPS_HEADER_3 => when RTPS_HEADER_3 =>
-- Input FIFO Guard -- Input FIFO Guard
if (empty = '0') then if (empty = '0') then
rd_sig <= '1'; rd_guard := '1';
cnt_next <= cnt + 1; cnt_next <= cnt + 1;
case (cnt) is case (cnt) is
@ -421,7 +428,7 @@ begin
when RTPS_SUB_HEADER => when RTPS_SUB_HEADER =>
-- Input FIFO Guard -- Input FIFO Guard
if (empty = '0') then if (empty = '0') then
rd_sig <= '1'; rd_guard := '1';
-- DEFAULT -- DEFAULT
src_is_reader_next <= '0'; 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), -- 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. -- 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 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
-- 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
if (rtps_sub_id /= SID_PAD and rtps_sub_id /= SID_INFO_TS) then -- Fix Submessage End Position
-- Fix Submessage End Position sub_end_next <= packet_length;
sub_end_next <= packet_length & "00";
end if;
else 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; end if;
case (rtps_sub_id) is case (rtps_sub_id) is
@ -494,7 +500,8 @@ begin
-- VALIDITY CHECK -- VALIDITY CHECK
elsif (rtps_sub_flags(SUBMESSAGE_DATA_FLAG_POS) = '1' and rtps_sub_flags(SUBMESSAGE_KEY_FLAG_POS) = '1') then 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) -- 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; end if;
-- PAD (Variable Size Padding) -- PAD (Variable Size Padding)
when SID_PAD => when SID_PAD =>
@ -507,7 +514,7 @@ begin
when PARSE_INFO_DST => when PARSE_INFO_DST =>
-- Input FIFO Guard -- Input FIFO Guard
if (empty = '0') then if (empty = '0') then
rd_sig <= '1'; rd_guard := '1';
cnt_next <= cnt + 1; cnt_next <= cnt + 1;
-- If Destination GUID Prefix is not us, skip the rest of the packet -- If Destination GUID Prefix is not us, skip the rest of the packet
@ -539,7 +546,7 @@ begin
when PARSE_INFO_SRC => when PARSE_INFO_SRC =>
-- Input FIFO Guard -- Input FIFO Guard
if (empty = '0') then if (empty = '0') then
rd_sig <= '1'; rd_guard := '1';
cnt_next <= cnt + 1; cnt_next <= cnt + 1;
case (cnt) is case (cnt) is
@ -549,7 +556,7 @@ begin
-- Protocol Version & Vendor ID -- Protocol Version & Vendor ID
when 1 => when 1 =>
-- Check Major Protocol Version -- 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 -- Protocol not supported, skip rest of Packet
stage_next <= SKIP_PACKET; stage_next <= SKIP_PACKET;
end if; end if;
@ -568,7 +575,7 @@ begin
end case; end case;
end if; end if;
when PARSE_INFO_TS => when PARSE_INFO_TS =>
rd_sig <= '1'; rd_guard := '1';
cnt_next <= cnt + 1; cnt_next <= cnt + 1;
case (cnt) is case (cnt) is
@ -587,7 +594,7 @@ begin
when PARSE_INFO_REPLY => when PARSE_INFO_REPLY =>
-- Input FIFO Guard -- Input FIFO Guard
if (empty = '0') then if (empty = '0') then
rd_sig <= '1'; rd_guard := '1';
-- Extract Locator List -- Extract Locator List
numlocators_next <= unsigned(data_in_swapped(numlocators_next'length-1 downto 0)); numlocators_next <= unsigned(data_in_swapped(numlocators_next'length-1 downto 0));
@ -611,7 +618,7 @@ begin
else else
-- Input FIFO Guard -- Input FIFO Guard
if (empty = '0') then if (empty = '0') then
rd_sig <= '1'; rd_guard := '1';
cnt_next <= cnt + 1; cnt_next <= cnt + 1;
case (cnt) is case (cnt) is
@ -631,7 +638,7 @@ begin
if (data_in_swapped(UDP_PORT_INVALID'range) = UDP_PORT_INVALID) then if (data_in_swapped(UDP_PORT_INVALID'range) = UDP_PORT_INVALID) then
locator_match_next <= '0'; locator_match_next <= '0';
else else
src_port_next <= data_in_swapped(src_port_next'length-1 downto 0); long_latch_next <= data_in_swapped;
end if; end if;
end if; end if;
-- Locator Address 1/4 -- Locator Address 1/4
@ -646,8 +653,9 @@ begin
-- Locator Address 4/4 (IPv4 Address) -- Locator Address 4/4 (IPv4 Address)
when 5 => when 5 =>
-- We only store valid UDPv4 Locators -- We only store valid UDPv4 Locators
if (locator_match = '1' and data_in_swapped /= IPv4_ADDRESS_INVALID) then if (locator_match = '1' and data_in /= IPv4_ADDRESS_INVALID) then
src_addr_next <= data_in_swapped; src_addr_next <= data_in;
src_port_next <= long_latch(src_port_next'length-1 downto 0);
end if; end if;
-- Last Word of Locator -- Last Word of Locator
numlocators_next <= numlocators - 1; numlocators_next <= numlocators - 1;
@ -660,7 +668,7 @@ begin
when PARSE_INFO_REPLY_IP4 => when PARSE_INFO_REPLY_IP4 =>
-- Input FIFO Guard -- Input FIFO Guard
if (empty = '0') then if (empty = '0') then
rd_sig <= '1'; rd_guard := '1';
cnt_next <= cnt + 1; cnt_next <= cnt + 1;
case (cnt) is case (cnt) is
@ -671,18 +679,21 @@ begin
locator_match_next <= '0'; locator_match_next <= '0';
else else
locator_match_next <= '1'; locator_match_next <= '1';
src_addr_next <= data_in_swapped; long_latch_next <= data_in_swapped;
end if; end if;
-- UDPv4 Port -- UDPv4 Port
when 1 => when 1 =>
-- Store only valid Locators -- Store only valid Locators
if (locator_match = '1' and data_in_swapped(UDP_PORT_INVALID'range) /= UDP_PORT_INVALID) then 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_port_next <= data_in_swapped(src_port_next'length-1 downto 0);
src_addr_next <= long_latch;
end if; end if;
-- Parse Multicast if available -- Parse Multicast if available
if (flags(SUBMESSAGE_MULTICAST_FLAG_POS) = '1') then 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; cnt_next <= 0;
else else
-- DONE -- DONE
@ -695,7 +706,7 @@ begin
when PARSE_HEARTBEAT => when PARSE_HEARTBEAT =>
-- Input FIFO Guard -- Input FIFO Guard
if (empty = '0') then if (empty = '0') then
rd_sig <= '1'; rd_guard := '1';
cnt_next <= cnt + 1; cnt_next <= cnt + 1;
case (cnt) is case (cnt) is
@ -752,7 +763,7 @@ begin
when PARSE_ACKNACK => when PARSE_ACKNACK =>
-- Input FIFO Guard -- Input FIFO Guard
if (empty = '0') then if (empty = '0') then
rd_sig <= '1'; rd_guard := '1';
cnt_next <= cnt + 1; cnt_next <= cnt + 1;
case (cnt) is case (cnt) is
@ -780,31 +791,29 @@ begin
-- ReaderSNState.NumBits -- ReaderSNState.NumBits
when 4 => when 4 =>
ulong_latch_next <= unsigned(data_in_swapped); 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));
bitmap_cnt_next <= unsigned(round_slv(data_in_swapped(log2c(MAX_BITMAP_WIDTH)-1 downto 0),bitmap_cnt'length)) - 1; cnt2_next <= 0;
-- VALIDITY CHECK -- VALIDITY CHECK
if (data_in_swapped(0) = '1' or unsigned(data_in_swapped) > 256) then 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 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) -- 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; 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; end if;
-- ReaderSNState.Bitmap -- ReaderSNState.Bitmap
when 5 => when 5 =>
cnt2_next <= cnt2 + 1; -- Read Bitmap
if (cnt2 < bitmap_cnt) then
bitmap_latch_next(cnt2) <= data_in_swapped; cnt2_next <= cnt2 + 1;
-- Keep Sub-State until whole Bitmap is read bitmap_latch_next(cnt2) <= data_in_swapped;
if (cnt2 /= bitmap_cnt) then
-- Keep Sub-State
cnt_next <= cnt; cnt_next <= cnt;
-- Exit Condition
else
-- Prevent Input Latching
rd_guard := '0';
end if; end if;
-- Count -- Count
when 6 => when 6 =>
@ -819,7 +828,7 @@ begin
when PARSE_GAP => when PARSE_GAP =>
-- Input FIFO Guard -- Input FIFO Guard
if (empty = '0') then if (empty = '0') then
rd_sig <= '1'; rd_guard := '1';
cnt_next <= cnt + 1; cnt_next <= cnt + 1;
case (cnt) is case (cnt) is
@ -834,7 +843,7 @@ begin
sn_latch_1_next(0) <= unsigned(data_in_swapped); sn_latch_1_next(0) <= unsigned(data_in_swapped);
-- GapStart Sequence Number 2/2 -- GapStart Sequence Number 2/2
when 3 => when 3 =>
sn_latch_1_next(0) <= unsigned(data_in_swapped); sn_latch_1_next(1) <= unsigned(data_in_swapped);
-- VALIDITY CHECK -- VALIDITY CHECK
tmp_sn := (0 => sn_latch_1(0), 1 => unsigned(data_in_swapped)); tmp_sn := (0 => sn_latch_1(0), 1 => unsigned(data_in_swapped));
@ -860,32 +869,29 @@ begin
-- ReaderSNState.NumBits -- ReaderSNState.NumBits
when 6 => when 6 =>
ulong_latch_next <= unsigned(data_in_swapped); 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));
bitmap_cnt_next <= unsigned(round_slv(data_in_swapped(log2c(MAX_BITMAP_WIDTH)-1 downto 0),bitmap_cnt'length)) - 1; cnt2_next <= 0;
-- VALIDITY CHECK -- VALIDITY CHECK
if (data_in_swapped(0) = '1' or unsigned(data_in_swapped) > 256) then 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 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) -- 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; 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; end if;
-- ReaderSNState.Bitmap -- ReaderSNState.Bitmap
when 7 => when 7 =>
cnt2_next <= cnt2 + 1; -- Read Bitmap
if (cnt2 < bitmap_cnt) then
bitmap_latch_next(cnt2) <= data_in_swapped; cnt2_next <= cnt2 + 1;
-- Keep Sub-State until whole Bitmap is read bitmap_latch_next(cnt2) <= data_in_swapped;
if (cnt2 /= bitmap_cnt) then
-- Keep Sub-State
cnt_next <= cnt; cnt_next <= cnt;
-- Exit Condition
else else
-- Prevent Input Latching
rd_guard := '0';
-- DONE -- DONE
stage_next <= MATCH_DST_ENDPOINT; stage_next <= MATCH_DST_ENDPOINT;
end if; end if;
@ -896,7 +902,7 @@ begin
when PARSE_DATA => when PARSE_DATA =>
-- Input FIFO Guard -- Input FIFO Guard
if (empty = '0') then if (empty = '0') then
rd_sig <= '1'; rd_guard := '1';
cnt_next <= cnt + 1; cnt_next <= cnt + 1;
case (cnt) is case (cnt) is
@ -905,7 +911,7 @@ begin
-- NOTE: Extra Flags are unused -- NOTE: Extra Flags are unused
-- Latch Length to skip Uknown Data Header Part and latch offset to ensure 4-Byte alignement (see align_prc) -- 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)); 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 -- Reader Entity ID
when 1 => when 1 =>
dest_entityid_next <= data_in; dest_entityid_next <= data_in;
@ -933,77 +939,71 @@ begin
end case; end case;
end if; end if;
when SKIP_DATA_HEADER => when SKIP_DATA_HEADER =>
if (empty = '0') then -- End of Data Header
-- End of Data Header if ((read_cnt & "00") >= data_header_end) then
if (read_cnt >= data_header_end) then stage_next <= MATCH_DST_ENDPOINT;
stage_next <= MATCH_DST_ENDPOINT; cnt_next <= 0;
cnt_next <= 0; -- Fix alignement
-- Fix alignement align_offset_next <= offset_latch;
align_offset_next <= offset_latch; -- Input Guard
-- Latch Input for alignment purposes elsif(empty = '0') then
align_sig_next <= data_in(23 downto 0); -- Latch Input for alignment purposes
else align_sig_next <= data_in(23 downto 0);
-- Skip-Read -- Skip-Read
rd_sig <= '1'; rd_guard := '1';
end if;
end if; end if;
when MATCH_DST_ENDPOINT => when MATCH_DST_ENDPOINT =>
-- Input FIFO Guard -- DEFAULT
if (empty = '0') then user_endpoint_next <= (others => '0');
rd_sig <= '1'; builtin_endpoint_next <= '0';
stage_next <= PUSH_PAYLOAD_HEADER;
-- DEFAULT cnt_next <= 0;
user_endpoint_next <= (others => '0');
builtin_endpoint_next <= '0'; -- *Check Dest Entity ID*
stage_next <= PUSH_PAYLOAD_HEADER; -- Target ALL Endpoints
cnt_next <= 0; if (dest_entityid = ENTITYID_UNKNOWN) then
if (is_metatraffic = '1') then
-- *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
builtin_endpoint_next <= '1'; 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 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; 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; end if;
when PUSH_PAYLOAD_HEADER => 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. -- 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; cnt2_next <= 0;
-- ReaderSNState.Bitmap -- ReaderSNState.Bitmap
when 3 => when 3 =>
cnt2_next <= cnt2 + 1; -- Write Bitmap
if (cnt2 < bitmap_cnt) then
data_out <= bitmap_latch_next(cnt2); cnt2_next <= cnt2 + 1;
wr_sig <= '1';
data_out <= bitmap_latch_next(cnt2);
-- Keep Sub-State until Bitmap is written wr_sig <= '1';
if (cnt2 /= bitmap_cnt) then
-- Keep Sub-State
cnt_next <= cnt; cnt_next <= cnt;
-- Exit Condition
end if; end if;
-- Count -- Count
when 4 => when 4 =>
@ -1171,14 +1173,16 @@ begin
cnt2_next <= 0; cnt2_next <= 0;
-- GapList.Bitmap -- GapList.Bitmap
when 5 => when 5 =>
cnt2_next <= cnt2 + 1; -- Write Bitmap
if (cnt2 < bitmap_cnt) then
data_out <= bitmap_latch_next(cnt2); cnt2_next <= cnt2 + 1;
wr_sig <= '1';
data_out <= bitmap_latch_next(cnt2);
-- Keep Sub-State until Bitmap is written wr_sig <= '1';
if (cnt2 /= bitmap_cnt) then
-- Keep Sub-State
cnt_next <= cnt; cnt_next <= cnt;
-- Exit Condition
else else
-- DONE -- DONE
stage_next <= SKIP_SUB; stage_next <= SKIP_SUB;
@ -1187,9 +1191,18 @@ begin
null; null;
end case; end case;
when SID_DATA => 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 -- Input FIFO Guard
if (empty = '0') then elsif (empty = '0') then
rd_sig <= '1'; rd_guard := '1';
-- Latch Input for alignment purposes -- Latch Input for alignment purposes
align_sig_next <= data_in(23 downto 0); align_sig_next <= data_in(23 downto 0);
@ -1197,15 +1210,6 @@ begin
-- Push Payload -- Push Payload
data_out <= data_in_aligned; data_out <= data_in_aligned;
wr_sig <= '1'; 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; end if;
when others => when others =>
stage_next <= SKIP_SUB; stage_next <= SKIP_SUB;
@ -1213,24 +1217,30 @@ begin
end if; end if;
when SKIP_SUB => when SKIP_SUB =>
-- End of Submessage -- End of Submessage
if ((read_cnt & "00") >= sub_end) then if (read_cnt = sub_end) then
-- Begin parsing of next submessage -- Begin parsing of next submessage
stage_next <= RTPS_SUB_HEADER; stage_next <= RTPS_SUB_HEADER;
-- Valid Submessage End -- Reset Submessage End
sub_end_next <= packet_length & "00"; sub_end_next <= (others => '1');
-- Input FIFO Guard -- Input FIFO Guard
elsif (empty = '0') then elsif (empty = '0') then
-- Skip-Read -- Skip-Read
rd_sig <= '1'; rd_guard := '1';
end if; end if;
when SKIP_PACKET => when SKIP_PACKET =>
-- Valid Submessage End -- Reset Submessage End
sub_end_next <= packet_length & "00"; 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 -- Input FIFO Guard
if (empty = '0') then elsif (empty = '0') then
-- Skip-Read -- Skip-Read
rd_sig <= '1'; rd_guard := '1';
end if; end if;
-- NOTE: Exit condition is via the OVERREAD GUARD -- NOTE: Exit condition is via the OVERREAD GUARD
when others => when others =>
@ -1238,29 +1248,37 @@ begin
end case; end case;
-- OVERREAD GUARD -- OVERREAD GUARD
-- Reached End of Packet -- XXX: Read from signal we set
if (read_cnt = packet_length) then -- 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 -- Force rd_sig low
rd_sig <= '0'; 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.
-- Notify Endpoints of EOP -- Notify Endpoints of EOP
last_word_out <= '1'; last_word_out <= '1';
-- Continue parsing next -- Continue parsing next Packet
stage_next <= SRC_ADDR_HEADER; stage_next <= SRC_ADDR_HEADER;
-- Reset packet_length until new packet_length is read (Prevent false positives) -- Reset Lengths
packet_length_next <= (others => '1'); packet_length_next <= (others => '1');
sub_end_next <= (others => '1');
-- Read outside of Submessage Length -- 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 -- 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 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. -- 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) -- Invalid Submessage Length Field, Skip Packet (see DDSI-RTPS 2.3 Section 8.3.4.1)
stage_next <= SKIP_PACKET; stage_next <= SKIP_PACKET;
-- Valid Submessage End -- Reset Submessage End
sub_end_next <= packet_length & "00"; sub_end_next <= (others => '1');
-- DEFAULT
else
rd_sig <= rd_guard;
end if; end if;
end process; end process;
@ -1271,10 +1289,12 @@ begin
if rising_edge(clk) then if rising_edge(clk) then
-- Reset Read counter -- Reset Read counter
if (reset = '1' or reset_read_cnt = '1') then 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 -- Increment read counter each time rd is high
elsif (rd_sig = '1') then 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 if; end if;
end process; end process;

84
src/rtps_test_package.vhd
View File

@ -1,8 +1,9 @@
library ieee; library ieee;
use ieee.std_logic_1164.all; use ieee.std_logic_1164.all;
use ieee.numeric_std.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.math_pkg.all;
use work.rtps_package.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); 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; procedure gen_rand_loc(RV : inout RandomPType; ret : out LOCATOR_TYPE);
impure function rand_slv(len : integer) return std_logic_vector;
impure function gen_rand_loc return LOCATOR_TYPE;
function int(n : integer; width : natural) return std_logic_vector; function int(n : integer; width : natural) return std_logic_vector;
end package; end package;
@ -202,34 +201,13 @@ end package;
package body rtps_test_package is package body rtps_test_package is
-- *UTILITY FUNCTIONS* -- *UTILITY FUNCTIONS*
impure function rand_int(min_val, max_val : integer) return integer is procedure gen_rand_loc(RV : inout RandomPType; ret : out LOCATOR_TYPE) 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;
begin begin
ret := EMPTY_LOCATOR;
ret.kind := LOCATOR_KIND_UDPv4; ret.kind := LOCATOR_KIND_UDPv4;
ret.portn(UDP_PORT_WIDTH-1 downto 0) := rand_slv(UDP_PORT_WIDTH); ret.portn(UDP_PORT_WIDTH-1 downto 0) := RV.RandSlv(UDP_PORT_WIDTH);
ret.addr(IPv4_ADDRESS_WIDTH-1 downto 0) := rand_slv(IPv4_ADDRESS_WIDTH); ret.addr(IPv4_ADDRESS_WIDTH-1 downto 0) := RV.RandSlv(IPv4_ADDRESS_WIDTH);
return ret; end procedure;
end function;
function int(n : integer; width : natural) return std_logic_vector is function int(n : integer; width : natural) return std_logic_vector is
begin begin
@ -238,9 +216,9 @@ package body rtps_test_package is
-- *DEFERRED DEFINITIONS* -- *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 := ( constant EMPTY_LOCATOR : LOCATOR_TYPE := (
kind => LOCATOR_KIND_INVALID, 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 procedure gen_udp_header(ref : in UDP_HEADER_TYPE; output : inout TEST_PACKET_TYPE) is
begin 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 -- 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.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; 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 procedure fix_udp_packet(output : inout TEST_PACKET_TYPE) is
begin begin
assert (output.length < 2**UDP_PORT_WIDTH) report "Exceeded maximum UDP Packet Size" severity error; 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 -- TODO: Calculate Checksum
end procedure; 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.data(output.length) := endian_swap(littleEndian, std_logic_vector(input.base(1)));
output.length := output.length + 1; output.length := output.length + 1;
-- Sequence Number State (NumBits) -- 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.data(output.length) := endian_swap(littleEndian, input.numBits);
output.length := output.length + 1; output.length := output.length + 1;
-- Sequence Number State (Bitmap) -- Sequence Number State (Bitmap)
@ -478,7 +468,7 @@ package body rtps_test_package is
output.data(output.length) := endian_swap(littleEndian, input.base); output.data(output.length) := endian_swap(littleEndian, input.base);
output.length := output.length + 1; output.length := output.length + 1;
-- Sequence Number State (NumBits) -- 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.data(output.length) := endian_swap(littleEndian, input.numBits);
output.length := output.length + 1; output.length := output.length + 1;
-- Sequence Number State (Bitmap) -- Sequence Number State (Bitmap)
@ -612,7 +602,7 @@ package body rtps_test_package is
tmp := to_integer(unsigned(ref.octetsToInlineQos)) - 16; tmp := to_integer(unsigned(ref.octetsToInlineQos)) - 16;
if (tmp > 0) then if (tmp > 0) then
for i in 0 to tmp-1 loop 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 if (i mod 4 = 3) then
output.length := output.length + 1; output.length := output.length + 1;
output.data(output.length) := (others => '0'); output.data(output.length) := (others => '0');
@ -630,7 +620,7 @@ package body rtps_test_package is
end loop; end loop;
end if; end if;
-- Fix Alignement -- 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; output.length := output.length + 1;
end if; end if;
end if; end if;
@ -652,17 +642,17 @@ package body rtps_test_package is
-- *INFO_REPLY_IP4* -- *INFO_REPLY_IP4*
elsif (ref.submessageID = SID_INFO_REPLY_IP4) then elsif (ref.submessageID = SID_INFO_REPLY_IP4) then
-- Unicast Locator (Address) -- 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; output.length := output.length + 1;
-- Unicast Locator (Port) -- 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; output.length := output.length + 1;
if (ref.flags(SUBMESSAGE_MULTICAST_FLAG_POS) = '1') then if (ref.flags(SUBMESSAGE_MULTICAST_FLAG_POS) = '1') then
-- Multicast Locator (Address) -- 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; output.length := output.length + 1;
-- Multicast Locator (Port) -- 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; output.length := output.length + 1;
end if; end if;
-- *INFO_TS* -- *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 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 -- Fix Submessage Length
if (ref.submessageLength = (ref.submessageLength'range => '1')) then 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 -- Fix Packet Length
else else
output.length := start + to_integer(unsigned(ref.submessageLength)); output.length := start + to_integer(unsigned(ref.submessageLength));
@ -699,7 +689,9 @@ package body rtps_test_package is
-- *PAD/UKNOWN* -- *PAD/UKNOWN*
else else
-- Padding -- 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.data(output.length) := (others => '0');
output.length := output.length + 1; output.length := output.length + 1;
end loop; end loop;
@ -749,9 +741,9 @@ package body rtps_test_package is
output.length := output.length + 1; output.length := output.length + 1;
-- Timestamp -- Timestamp
if (not is_meta) then 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.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; output.length := output.length + 1;
end if; end if;
-- DATA PAYLOAD -- DATA PAYLOAD