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

* Added Documentation in RTPS

Browse Source
This commit is contained in:
Greek 2020-05-24 18:28:57 +02:00
parent 70ace14c6b
commit 74f404834e
2 changed files with 144 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

184
src/rtps_handler.vhd
View File

@ -9,43 +9,55 @@ use work.rtps_package.all;
entity rtps_handler is entity rtps_handler is
port ( port (
clk : in std_logic; -- Input Clock clk : in std_logic; -- Input Clock
reset : in std_logic; -- Synchronous Reset reset : in std_logic; -- Synchronous Reset
empty : in std_logic; -- Input FIFO empty flag empty : in std_logic; -- Input FIFO empty flag
rd : out std_logic; -- Input FIFO read signal rd : out std_logic; -- Input FIFO read signal
data_in : in std_logic_vector(31 downto 0); -- Input FIFO data signal data_in : in std_logic_vector(31 downto 0); -- Input FIFO data signal
builtin_output : out BUILTIN_ENDPOINT_TYPE; -- Output FIFO (Built-In Endpoints) data signal
builtin_output : out BUILTIN_ENDPOINT_TYPE; builtin_full : in std_logic_vector(NUM_DOMAIN-1 downto 0); -- Output FIFO (Built-In Endpoints) full signal
builtin_full : in std_logic_vector(NUM_DOMAIN-1 downto 0); builtin_wr : out std_logic_vector(NUM_DOMAIN-1 downto 0); -- Output FIFO (Built-In Endpoints) write signal
builtin_wr : out std_logic_vector(NUM_DOMAIN-1 downto 0); user_output : out USER_ENDPOINT_OUTPUT; -- Output FIFO (User Endpoints) data signal
user_output : out USER_ENDPOINT_OUTPUT; user_full : in std_logic_vector(MAX_ENDPOINTS-1 downto 0); -- Output FIFO (User Endpoints) full signal
user_full : in std_logic_vector(NUM_READERS+NUM_WRITERS-1 downto 0); user_wr : out std_logic_vector(MAX_ENDPOINTS-1 downto 0) -- Output FIFO (User Endpoints) write signal
user_wr : out std_logic_vector(NUM_READERS+NUM_WRITERS-1 downto 0)
); );
end entity; end entity;
architecture arch of rtps_handler is architecture arch of rtps_handler is
--*****COMPOENENT DECLARATION****** --*****COMPONENT DECLARATION******
--*****CONSTANT DECLARATION***** --*****CONSTANT DECLARATION*****
-- Minimum Packet Length to consider valid -- Minimum Packet Length to consider valid (32-bit Words)
-- 2 UDP Header 32-bit Words, 5 RTPS Header 32-bit Words -- 2 UDP Header, 5 RTPS Header
constant MIN_PACKET_LENGTH : integer := 7; constant MIN_PACKET_LENGTH : integer := 7;
constant MAX_ENDPOINTS : integer := NUM_READERS+NUM_WRITERS; -- Minimum ACKNACK Submessage Size (Bytes)
-- 4 Reader ID, 4 Writer ID, 12 SequenceNumberSet (8 BitmapBase, 4 numBits), 4 Count
constant MIN_ACKNACK_SIZE : integer := 24; constant MIN_ACKNACK_SIZE : integer := 24;
-- Minimum INFO_DST Submessage Size (Bytes)
-- 12 GUID Prefix
constant MIN_INFO_DST_SIZE : integer := 12; constant MIN_INFO_DST_SIZE : integer := 12;
-- Minimum INFO_SRC Submessage Size (Bytes)
-- 4 unused, 2 ProtocolVersion, 2 VendorID, 12 GUID Prefix
constant MIN_INFO_SRC_SIZE : integer := 20; constant MIN_INFO_SRC_SIZE : integer := 20;
constant MIN_INFO_REPLY_SIZE : integer := 1; -- Minimum INFO_REPLY Submessage Size (Bytes)
constant MIN_INFO_REPLY_IP4_SIZE: integer := 2; -- 4 Unicast LocatorList (4 numLocators)
constant MIN_INFO_REPLY_SIZE : integer := 4;
-- Minimum INFO_REPLY_IP4 Submessage Size (Bytes)
-- 8 Unicast LocatorUPDv4 (4 Addr, 4 Port)
constant MIN_INFO_REPLY_IP4_SIZE: integer := 8;
-- Minimum GAP Submessage Size (Bytes)
-- 4 Reader ID, 4 Writer ID, 8 SequenceNumber, 12 SequenceNumberSet (8 BitmapBase, 4 numBits)
constant MIN_GAP_SIZE : integer := 28; constant MIN_GAP_SIZE : integer := 28;
-- Minimum HEARTBEAT Submessage Size (Bytes)
-- 4 Reader ID, 4 Writer ID, 8 SequenceNumber, 8 SequenceNumber, 4 Count
constant MIN_HEARTBEAT_SIZE : integer := 28; constant MIN_HEARTBEAT_SIZE : integer := 28;
-- Minimum DATA Submessage Size (Bytes)
-- 2 Extra Flags, 2 octetstoinlineQoS, 4 Reader ID, 4 Writer ID, 8 SequenceNumber
constant MIN_DATA_SIZE : integer := 20; constant MIN_DATA_SIZE : integer := 20;
--GUIDPREFIX(1 downto 0) <= VENDORID;
--*****TYPE DECLARATION***** --*****TYPE DECLARATION*****
-- FSM states. Explained below in detail
type STAGE_TYPE is (SRC_ADDR_HEADER, DEST_ADDR_HEADER, LEN_HEADER, UDP_HEADER_1, UDP_HEADER_2, RTPS_HEADER_1, RTPS_HEADER_2, type STAGE_TYPE is (SRC_ADDR_HEADER, DEST_ADDR_HEADER, LEN_HEADER, UDP_HEADER_1, UDP_HEADER_2, RTPS_HEADER_1, RTPS_HEADER_2,
RTPS_HEADER_3, RTPS_SUB_HEADER, EXTRACT_LOCATOR_UDPv4_1, EXTRACT_LOCATOR_UDPv4_2, RTPS_HEADER_3, RTPS_SUB_HEADER, EXTRACT_LOCATOR_UDPv4_1, EXTRACT_LOCATOR_UDPv4_2,
EXTRACT_LOCATOR_LIST, EXTRACT_LOCATOR, INFO_SRC_HEADER, EXTRACT_DOMAIN_ID, SRC_ENTPOINT, DATA_HEADER, EXTRACT_LOCATOR_LIST, EXTRACT_LOCATOR, INFO_SRC_HEADER, EXTRACT_DOMAIN_ID, SRC_ENTPOINT, DATA_HEADER,
@ -56,45 +68,68 @@ architecture arch of rtps_handler is
--*****SIGNAL DECLARATION***** --*****SIGNAL DECLARATION*****
-- FSM state -- FSM state
signal stage, stage_next : STAGE_TYPE := SRC_ADDR_HEADER; signal stage, stage_next : STAGE_TYPE := SRC_ADDR_HEADER;
-- FSM state latch. Used to transition dynamically to different states from the same state.
signal return_stage, return_stage_next : STAGE_TYPE := SRC_ADDR_HEADER; signal return_stage, return_stage_next : STAGE_TYPE := SRC_ADDR_HEADER;
-- Intermediate input read signal. (Read from output port not allowed) -- Intermediate input read signal. (Read from output port not allowed)
signal rd_sig : std_logic := '0'; signal rd_sig : std_logic := '0';
-- Signal used to reset the word counter -- Signal used to reset the word counter
signal reset_read_cnt : std_logic; signal reset_read_cnt : std_logic;
-- 32-bit word counter (Counts words read from input fifo) -- 32-bit Word counter (Counts words read from input fifo)
signal read_cnt : unsigned(13 downto 0) := (others => '0'); signal read_cnt : unsigned(13 downto 0) := (others => '0');
-- 32-bit aligned total packet length -- 32-bit Word aligned total packet length
signal packet_length, packet_length_next : unsigned(13 downto 0) := (others => '0'); signal packet_length, packet_length_next : unsigned(13 downto 0) := (others => '0');
-- 32-bit Word aligned end of Submessage
signal sub_end, sub_end_next : unsigned(13 downto 0) := (others => '0'); signal sub_end, sub_end_next : unsigned(13 downto 0) := (others => '0');
-- 32-bit Word aligned end of Data Header (Beginning of inlineQoS/Payload)
signal data_header_end, data_header_end_next : unsigned(13 downto 0) := (others => '0'); signal data_header_end, data_header_end_next : unsigned(13 downto 0) := (others => '0');
-- Input Signal Latch. Used to read 32-bit Word aligned from input
signal align_sig, align_sig_next : std_logic_vector(23 downto 0) := (others => '0'); signal align_sig, align_sig_next : std_logic_vector(23 downto 0) := (others => '0');
-- 32-bit Word Aligned Input
signal aligned_data_in : std_logic_vector(31 downto 0); signal aligned_data_in : std_logic_vector(31 downto 0);
-- 32-bit Word Aligned Input, Padded with Zeroes (The last read from input with less than 32-bits gets padded with zeroes)
signal aligned_data_in_padded : std_logic_vector(31 downto 0) := (others => '0'); signal aligned_data_in_padded : std_logic_vector(31 downto 0) := (others => '0');
-- 32-bit Word alignement offset
signal align_offset, align_offset_next : std_logic_vector(1 downto 0) := (others => '0'); signal align_offset, align_offset_next : std_logic_vector(1 downto 0) := (others => '0');
-- Alignement offset latch
signal offset_latch, offset_latch_next : std_logic_vector(1 downto 0) := (others => '0'); signal offset_latch, offset_latch_next : std_logic_vector(1 downto 0) := (others => '0');
-- Alignement offset latch (For Data Header)
signal data_header_offset_latch, data_header_offset_latch_next : std_logic_vector(1 downto 0) := (others => '0'); signal data_header_offset_latch, data_header_offset_latch_next : std_logic_vector(1 downto 0) := (others => '0');
-- IPv4 Source Address latch
signal src_addr, src_addr_next : std_logic_vector(31 downto 0) := (others => '0'); signal src_addr, src_addr_next : std_logic_vector(31 downto 0) := (others => '0');
-- UDP Source Port latch
signal src_port, src_port_next : std_logic_vector(15 downto 0) := (others => '0'); signal src_port, src_port_next : std_logic_vector(15 downto 0) := (others => '0');
-- Denotes if processed Message has Multicast Destination
signal is_multicast, is_multicast_next : std_logic := '0'; signal is_multicast, is_multicast_next : std_logic := '0';
-- Denotes if processed Message is Metatraffic destined for Built-In Endpoints
signal is_metatraffic, is_metatraffic_next : std_logic := '0'; signal is_metatraffic, is_metatraffic_next : std_logic := '0';
-- Domain ID/Participant ID target of processed Message
signal domain_id, domain_id_next : integer range 0 to NUM_DOMAIN-1 := 0; signal domain_id, domain_id_next : integer range 0 to NUM_DOMAIN-1 := 0;
signal endpoint_id, endpoint_id_next : integer range 0 to MAX_ENDPOINTS-1 := 0; -- RTPS Submessage Flag latch
signal flags, flags_next : std_logic_vector(7 downto 0) := (others => '0'); signal flags, flags_next : std_logic_vector(7 downto 0) := (others => '0');
-- Source Endpoint Entity ID latch
signal src_entityid, src_entityid_next : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0'); signal src_entityid, src_entityid_next : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0');
-- Vector denoting the Destination User Endpoints of the Message
signal user_endpoint, user_endpoint_next : std_logic_vector(MAX_ENDPOINTS-1 downto 0) := (others => '0'); signal user_endpoint, user_endpoint_next : std_logic_vector(MAX_ENDPOINTS-1 downto 0) := (others => '0');
-- Denoting if the Message is destined for the Built-in Endpoints
signal builtin_endpoint, builtin_endpoint_next : std_logic := '0'; signal builtin_endpoint, builtin_endpoint_next : std_logic := '0';
signal sub_length, sub_length_next : std_logic_vector(13 downto 0) := (others => '0'); -- Length of Payload (32-bit Words) to be sent to Endpoints
-- Since Submessages are limited to 2^16 Bytes, we can limit this counter also to 16 bits signal payload_length, payload_length_next : std_logic_vector(13 downto 0) := (others => '0');
-- numLocator latch
-- NOTE: Since Submessages are limited to 2^16 Bytes, we can limit this also to 16 bits
signal numlocators, numlocators_next : unsigned(15 downto 0) := (others => '0'); signal numlocators, numlocators_next : unsigned(15 downto 0) := (others => '0');
-- Denotes if a suitable Locator has been found
signal locator_match, locator_match_next : std_logic := '0'; signal locator_match, locator_match_next : std_logic := '0';
-- Denotes if the Source of the Message is a Reader Endpoint
signal src_is_reader, src_is_reader_next : std_logic := '0'; signal src_is_reader, src_is_reader_next : std_logic := '0';
-- Intermediate Output Data Signal
signal output_sig : std_logic_vector(31 downto 0) := (others => '0'); signal output_sig : std_logic_vector(31 downto 0) := (others => '0');
-- Intermediate Write Enable Signal
signal wr_sig : std_logic := '0'; signal wr_sig : std_logic := '0';
-- Submessage ID latch (Used as OPCODe by Endpoints)
signal opcode, opcode_next : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := (others => '0'); signal opcode, opcode_next : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := (others => '0');
-- Denotes if Message is DATA Submessage
signal is_data, is_data_next : std_logic := '0'; signal is_data, is_data_next : std_logic := '0';
signal data_header_length, data_header_length_next : unsigned(15 downto 0) := (others => '0'); -- General Purpose counter
-- General purpose counter
signal cnt, cnt_next : integer range 0 to max(GUIDPREFIX_WIDTH/32, 6) := 0; signal cnt, cnt_next : integer range 0 to max(GUIDPREFIX_WIDTH/32, 6) := 0;
--*****ALIAS DEFINATION***** --*****ALIAS DEFINATION*****
@ -220,16 +255,19 @@ architecture arch of rtps_handler is
end function; end function;
begin begin
rd <= rd_sig;
-- This process connects the Intermediate Output Signals to the actual output FIFOs
output_prc : process(all) output_prc : process(all)
begin begin
rd <= rd_sig; -- Data Signal
for i in 0 to NUM_DOMAIN-1 loop for i in 0 to NUM_DOMAIN-1 loop
builtin_output(i) <= output_sig; builtin_output(i) <= output_sig;
end loop; end loop;
for i in 0 to MAX_ENDPOINTS-1 loop for i in 0 to MAX_ENDPOINTS-1 loop
user_output(i) <= output_sig; user_output(i) <= output_sig;
end loop; end loop;
--Write Enable Signal
builtin_wr <= (others => '0'); builtin_wr <= (others => '0');
user_wr <= (others => '0'); user_wr <= (others => '0');
if (wr_sig = '1') then if (wr_sig = '1') then
@ -241,6 +279,12 @@ begin
end if; end if;
end process; end process;
-- This process is responsible for reading the input FIFO 32-bit Word aligned.
-- Since the RTPS Header uses octet Length counters, the Elements are not guaranteed to be 32-bit
-- aligned (Which is how we read from the input FIFO).
-- We store the lower 2 bits of the octet length (Which denotes the 32-bit alignment offset),
-- and at the end of the Byte aligned Element we pad it to 32-bit, and calculate the new
-- alignement of the data following (Done by adding the current offset to the previous offset)
align_prc : process(all) align_prc : process(all)
variable input : std_logic_vector(55 downto 0) := (others => '0'); variable input : std_logic_vector(55 downto 0) := (others => '0');
begin begin
@ -257,13 +301,38 @@ begin
end case; end case;
end process; end process;
-- Main State Machine
-- STATE DESCRIPTION
-- SRC_ADDR_HEADER Initial and Idle state. Read IPv4 Source Address
-- DEST_ADDR_HEADER Read IPv4 Destination Address
-- LEN_HEADER Read UDP Packet Length
-- UDP_HEADER_1 Parse first word of UDP Header
-- UDP_HEADER_2 Parse second word of UDP Header
-- RTPS_HEADER_1 Parse first word of RTPS Header
-- RTPS_HEADER_2 Parse second word of RTPS Header
-- RTPS_HEADER_3 Parse GUID Prefix of RTPS Header (3rd-5th Word)
-- RTPS_SUB_HEADER Parse RTPS Submessage Header
-- EXTRACT_LOCATOR_UDPv4_1 Read IPv4 Address from LocatorUDPv4
-- EXTRACT_LOCATOR_UDPv4_2 Read UDP Port from LocatorUDPv4
-- EXTRACT_LOCATOR_LIST Parse LocatorList Head
-- EXTRACT_LOCATOR Parse Locator
-- INFO_SRC_HEADER Parse INFO_SRC content
-- EXTRACT_DOMAIN_ID Extract Domain ID from GUID Prefix (Works only for GUID Prefixes generated by rtps_package)
-- SRC_ENTPOINT Read EntityID
-- DATA_HEADER Parse Submessage DATA Subheader
-- MATCH_DST_ENDPOINT Determine destination of Submessage
-- PUSH_PAYLOAD_HEADER Write Payload Header into relevant output FIFOs
-- DATA_SKIP_HEADER Read known DATA Submessage Subheader Elements, and skip the rest
-- PUSH_PAYLOAD Read from input FIFO into relevant output FIFOs
-- CHECK_SUB_END Check if end of Submessage is reached, and handle accordingly
-- SKIP_PACKET Skip rest of UDP Packet
-- SKIP_SUB Skip rest of Submessage
parse_prc: process(all) parse_prc: process(all)
variable tmp : integer range 0 to MAX_ENDPOINTS := 0; variable tmp : integer range 0 to MAX_ENDPOINTS := 0;
variable tmp_length : std_logic_vector(15 downto 0) := (others => '0'); variable tmp_length : std_logic_vector(15 downto 0) := (others => '0');
begin begin
--DEFAULT --DEFAULT Registered
stage_next <= stage; stage_next <= stage;
reset_read_cnt <= '0';
cnt_next <= cnt; cnt_next <= cnt;
align_offset_next <= align_offset; align_offset_next <= align_offset;
align_sig_next <= align_sig; align_sig_next <= align_sig;
@ -271,9 +340,7 @@ begin
sub_end_next <= sub_end; sub_end_next <= sub_end;
offset_latch_next <= offset_latch; offset_latch_next <= offset_latch;
src_addr_next <= src_addr; src_addr_next <= src_addr;
is_multicast_next <= is_multicast;
src_port_next <= src_port; src_port_next <= src_port;
is_metatraffic_next <= is_metatraffic;
domain_id_next <= domain_id; domain_id_next <= domain_id;
flags_next <= flags; flags_next <= flags;
src_entityid_next <= src_entityid; src_entityid_next <= src_entityid;
@ -281,16 +348,20 @@ begin
builtin_endpoint_next <= builtin_endpoint_next; builtin_endpoint_next <= builtin_endpoint_next;
return_stage_next <= return_stage; return_stage_next <= return_stage;
numlocators_next <= numlocators; numlocators_next <= numlocators;
locator_match_next <= locator_match; payload_length_next <= payload_length;
sub_length_next <= sub_length;
src_is_reader_next <= src_is_reader;
output_sig <= (others => '0');
wr_sig <= '0';
opcode_next <= opcode; opcode_next <= opcode;
data_header_end_next <= data_header_end; data_header_end_next <= data_header_end;
data_header_offset_latch_next <= data_header_offset_latch; data_header_offset_latch_next <= data_header_offset_latch;
locator_match_next <= locator_match;
is_metatraffic_next <= is_metatraffic;
is_multicast_next <= is_multicast;
is_data_next <= is_data; is_data_next <= is_data;
data_header_length_next <= data_header_length; src_is_reader_next <= src_is_reader;
-- DEFAULT Unregistered
rd_sig <= '0';
reset_read_cnt <= '0';
output_sig <= (others => '0');
wr_sig <= '0';
case(stage) is case(stage) is
-- Initial/Idle State -- Initial/Idle State
@ -323,6 +394,7 @@ begin
stage_next <= SKIP_PACKET; stage_next <= SKIP_PACKET;
end if; end if;
end if; end if;
-- UDP Packet Length
when LEN_HEADER => when LEN_HEADER =>
-- Reset packet Byte Counter -- Reset packet Byte Counter
reset_read_cnt <= '1'; reset_read_cnt <= '1';
@ -458,6 +530,7 @@ begin
case (rtps_sub_id) is case (rtps_sub_id) is
-- INFO_DST (Writer -> Reader, Update Destination GUID Prefix) -- INFO_DST (Writer -> Reader, Update Destination GUID Prefix)
-- STAGE ORDER: EXTRACT_DOMAIN_ID -> CHECK_SUB_END -> (SKIP_SUB) -> RTPS_SUB_HEADER
when SID_INFO_DST => when SID_INFO_DST =>
-- Check Length -- Check Length
if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_INFO_DST_SIZE)) then if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_INFO_DST_SIZE)) then
@ -471,6 +544,7 @@ begin
return_stage_next <= CHECK_SUB_END; return_stage_next <= CHECK_SUB_END;
end if; end if;
-- INFO_SRC (RTPS Header in Submessage form) -- INFO_SRC (RTPS Header in Submessage form)
-- STAGE ORDER: INFO_SRC_HEADER -> SKIP_SUB -> RTPS_SUB_HEADER
when SID_INFO_SRC => when SID_INFO_SRC =>
-- Check Length -- Check Length
if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_INFO_SRC_SIZE)) then if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_INFO_SRC_SIZE)) then
@ -483,10 +557,12 @@ begin
stage_next <= INFO_SRC_HEADER; stage_next <= INFO_SRC_HEADER;
end if; end if;
-- INFO_TS (Source Timestamp) -- INFO_TS (Source Timestamp)
-- STAGE ORDER: SKIP_SUB -> RTPS_SUB_HEADER
when SID_INFO_TS => when SID_INFO_TS =>
-- IGNORE -- IGNORE
stage_next <= CHECK_SUB_END; stage_next <= CHECK_SUB_END;
-- INFO_REPLY (Source Port and Address) -- INFO_REPLY (Source Port and Address)
-- STAGE ORDER: EXTRACT_LOCATOR_LIST <-> EXTRACT_LOCATOR -> CHECK_SUB_END -> (SKIP_SUB) -> RTPS_SUB_HEADER
when SID_INFO_REPLY => when SID_INFO_REPLY =>
-- Check Length -- Check Length
if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_INFO_REPLY_SIZE)) then if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_INFO_REPLY_SIZE)) then
@ -497,6 +573,7 @@ begin
stage_next <= EXTRACT_LOCATOR_LIST; stage_next <= EXTRACT_LOCATOR_LIST;
end if; end if;
-- INFO_REPLY (Source Port and Address) -- INFO_REPLY (Source Port and Address)
-- STAGE ORDER: EXTRACT_LOCATOR_UDPv4_1 -> EXTRACT_LOCATOR_UDPv4_2 -> CHECK_SUB_END -> (SKIP_SUB) -> RTPS_SUB_HEADER
when SID_INFO_REPLY_IP4 => when SID_INFO_REPLY_IP4 =>
-- Check Length -- Check Length
if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_INFO_REPLY_IP4_SIZE)) then if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_INFO_REPLY_IP4_SIZE)) then
@ -504,9 +581,10 @@ begin
stage_next <= SKIP_PACKET; stage_next <= SKIP_PACKET;
else else
-- Parse Locators -- Parse Locators
stage_next <= SKIP_SUB; stage_next <= EXTRACT_LOCATOR_UDPv4_1;
end if; end if;
-- Heartbeat (Writer -> Reader, Available SeqNum) -- Heartbeat (Writer -> Reader, Available SeqNum)
-- STAGE ORDER: MATCH_DST_ENDPOINT -> SRC_ENTPOINT -> PUSH_PAYLOAD_HEADER -> PUSH_PAYLOAD -> RTPS_SUB_HEADER
when SID_HEARTBEAT => when SID_HEARTBEAT =>
-- Check Length -- Check Length
if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_HEARTBEAT_SIZE)) then if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_HEARTBEAT_SIZE)) then
@ -518,6 +596,7 @@ begin
return_stage_next <= SRC_ENTPOINT; return_stage_next <= SRC_ENTPOINT;
end if; end if;
-- AckNack (Reader -> Writer, Request SeqNum) -- AckNack (Reader -> Writer, Request SeqNum)
-- STAGE ORDER: SRC_ENTPOINT -> MATCH_DST_ENDPOINT -> PUSH_PAYLOAD_HEADER -> PUSH_PAYLOAD -> RTPS_SUB_HEADER
when SID_ACKNACK => when SID_ACKNACK =>
-- Check Length -- Check Length
if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_ACKNACK_SIZE)) then if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_ACKNACK_SIZE)) then
@ -530,6 +609,7 @@ begin
stage_next <= SRC_ENTPOINT; stage_next <= SRC_ENTPOINT;
end if; end if;
-- GAP (Writer -> Reader, Invalidate SeqNum) -- GAP (Writer -> Reader, Invalidate SeqNum)
-- STAGE ORDER: MATCH_DST_ENDPOINT -> SRC_ENTPOINT -> PUSH_PAYLOAD_HEADER -> PUSH_PAYLOAD -> RTPS_SUB_HEADER
when SID_GAP => when SID_GAP =>
-- Check Length -- Check Length
if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_GAP_SIZE)) then if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_GAP_SIZE)) then
@ -541,6 +621,7 @@ begin
return_stage_next <= SRC_ENTPOINT; return_stage_next <= SRC_ENTPOINT;
end if; end if;
-- DATA (Writer -> Reader, SeqNum+Data) -- DATA (Writer -> Reader, SeqNum+Data)
-- STAGE ORDER: DATA_HEADER -> MATCH_DST_ENDPOINT -> SRC_ENTPOINT -> PUSH_PAYLOAD_HEADER -> DATA_SKIP_HEADER -> PUSH_PAYLOAD -> RTPS_SUB_HEADER
when SID_DATA => when SID_DATA =>
-- Check Length -- Check Length
if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_DATA_SIZE)) then if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_DATA_SIZE)) then
@ -569,6 +650,7 @@ begin
-- IGNORE -- IGNORE
stage_next <= SKIP_SUB; stage_next <= SKIP_SUB;
-- PAD (Variable Size Padding) -- PAD (Variable Size Padding)
-- STAGE ORDER: CHECK_SUB_END -> (SKIP_PACKET) -> RTPS_SUB_HEADER
when SID_PAD => when SID_PAD =>
stage_next <= CHECK_SUB_END; stage_next <= CHECK_SUB_END;
-- Unknown ID, skip submessage -- Unknown ID, skip submessage
@ -590,7 +672,7 @@ begin
sub_end_next <= packet_length; sub_end_next <= packet_length;
-- Calculate Submessage Length -- Calculate Submessage Length
-- TODO: Check synthesized code (Should be Subtractor/Adder with carry in) -- TODO: Check synthesized code (Should be Subtractor/Adder with carry in)
sub_length_next <= std_logic_vector(packet_length - read_cnt + to_unsigned(1,sub_length'length)); payload_length_next <= std_logic_vector(packet_length - read_cnt + to_unsigned(1,payload_length'length));
end if; end if;
else else
-- Latch Submessage End -- Latch Submessage End
@ -600,10 +682,10 @@ begin
-- Latch Submessage Size (+1 to adjust to the format recognised by the Endpoints) -- Latch Submessage Size (+1 to adjust to the format recognised by the Endpoints)
if (rtps_sub_id = SID_DATA) then if (rtps_sub_id = SID_DATA) then
-- (+5 to counter Data header subtraction) -- (+5 to counter Data header subtraction)
sub_length_next <= std_logic_vector(unsigned(normalize_length(endian_swap(rtps_sub_endianness,rtps_sub_length))) + to_unsigned(5,sub_length'length)); payload_length_next <= std_logic_vector(unsigned(normalize_length(endian_swap(rtps_sub_endianness,rtps_sub_length))) + to_unsigned(5,payload_length'length));
else else
-- (+1 to adjust to the format recognised by the Endpoints) -- (+1 to adjust to the format recognised by the Endpoints)
sub_length_next <= std_logic_vector(unsigned(normalize_length(endian_swap(rtps_sub_endianness,rtps_sub_length))) + to_unsigned(1,sub_length'length)); payload_length_next <= std_logic_vector(unsigned(normalize_length(endian_swap(rtps_sub_endianness,rtps_sub_length))) + to_unsigned(1,payload_length'length));
end if; end if;
end if; end if;
-- Latch Byte offset of next Header -- Latch Byte offset of next Header
@ -758,7 +840,7 @@ begin
-- Latch offset -- Latch offset
data_header_end_next <= read_cnt + unsigned(normalize_length(endian_swap(rtps_sub_endianness, rtps_sub_data_length))); data_header_end_next <= read_cnt + unsigned(normalize_length(endian_swap(rtps_sub_endianness, rtps_sub_data_length)));
-- Fix Payload Size -- Fix Payload Size
sub_length_next <= std_logic_vector(unsigned(sub_length) - unsigned(normalize_length(endian_swap(rtps_sub_endianness, rtps_sub_data_length)))); payload_length_next <= std_logic_vector(unsigned(payload_length) - unsigned(normalize_length(endian_swap(rtps_sub_endianness, rtps_sub_data_length))));
-- Next Stage -- Next Stage
stage_next <= MATCH_DST_ENDPOINT; stage_next <= MATCH_DST_ENDPOINT;
return_stage_next <= SRC_ENTPOINT; return_stage_next <= SRC_ENTPOINT;
@ -825,8 +907,8 @@ begin
cnt_next <= cnt + 1; cnt_next <= cnt + 1;
case (cnt) is case (cnt) is
when 1 => when 1 =>
output_sig(sub_length'length-1 downto 0) <= sub_length; output_sig(payload_length'length-1 downto 0) <= payload_length;
wr_sig <= '1'; wr_sig <= '1';
when 2 => when 2 =>
output_sig <= opcode & flags & src_port; output_sig <= opcode & flags & src_port;
wr_sig <= '1'; wr_sig <= '1';
@ -985,11 +1067,10 @@ begin
src_entityid <= (others => '0'); src_entityid <= (others => '0');
user_endpoint <= (others => '0'); user_endpoint <= (others => '0');
numlocators <= (others => '0'); numlocators <= (others => '0');
sub_length <= (others => '0'); payload_length <= (others => '0');
opcode <= (others => '0'); opcode <= (others => '0');
data_header_end <= (others => '0'); data_header_end <= (others => '0');
data_header_offset_latch <= (others => '0'); data_header_offset_latch <= (others => '0');
data_header_length <= (others => '0');
is_data <= '0'; is_data <= '0';
src_is_reader <= '0'; src_is_reader <= '0';
locator_match <= '0'; locator_match <= '0';
@ -1012,11 +1093,10 @@ begin
src_entityid <= src_entityid_next; src_entityid <= src_entityid_next;
user_endpoint <= user_endpoint_next; user_endpoint <= user_endpoint_next;
numlocators <= numlocators_next; numlocators <= numlocators_next;
sub_length <= sub_length_next; payload_length <= payload_length_next;
opcode <= opcode_next; opcode <= opcode_next;
data_header_end <= data_header_end_next; data_header_end <= data_header_end_next;
data_header_offset_latch <= data_header_offset_latch_next; data_header_offset_latch <= data_header_offset_latch_next;
data_header_length <= data_header_length_next;
is_data <= is_data_next; is_data <= is_data_next;
src_is_reader <= src_is_reader_next; src_is_reader <= src_is_reader_next;
locator_match <= locator_match_next; locator_match <= locator_match_next;

17
src/rtps_package.vhd
View File

@ -11,6 +11,11 @@ package rtps_package is
constant NUM_WRITERS : integer := 0; constant NUM_WRITERS : integer := 0;
-- Number of RTPS Reader Endpoints -- Number of RTPS Reader Endpoints
constant NUM_READERS : integer := 1; constant NUM_READERS : integer := 1;
-----------------------------------------------------------------------------------------------------
-- *DO NOT MODIFY BEGIN*
constant MAX_ENDPOINTS : integer := NUM_READERS+NUM_WRITERS;
-- *DO NOT MODIFY END*
-----------------------------------------------------------------------------------------------------
-- PB Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1) -- PB Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)
constant PORT_CONFIG_PB : integer := 7400; constant PORT_CONFIG_PB : integer := 7400;
-- DG Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1) -- DG Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)
@ -39,8 +44,8 @@ package rtps_package is
----------------------------------------------------------------------------------------------------- -----------------------------------------------------------------------------------------------------
-- *DO NOT MODIFY BEGIN* -- *DO NOT MODIFY BEGIN*
type ENDPOINT_DOMAIN_MAP_TYPE is array (NUM_READERS+NUM_WRITERS-1 downto 0) of integer; type ENDPOINT_DOMAIN_MAP_TYPE is array (MAX_ENDPOINTS-1 downto 0) of integer;
type ENDPOINT_WITH_KEY_TYPE is array (NUM_READERS+NUM_WRITERS-1 downto 0) of boolean; type ENDPOINT_WITH_KEY_TYPE is array (MAX_ENDPOINTS-1 downto 0) of boolean;
-- *DO NOT MODIFY END* -- *DO NOT MODIFY END*
----------------------------------------------------------------------------------------------------- -----------------------------------------------------------------------------------------------------
@ -91,7 +96,7 @@ package rtps_package is
type DOMAIN_ID_TYPE is array (NUM_DOMAIN-1 downto 0) of std_logic_vector(DOMAIN_ID_WIDTH-1 downto 0); type DOMAIN_ID_TYPE is array (NUM_DOMAIN-1 downto 0) of std_logic_vector(DOMAIN_ID_WIDTH-1 downto 0);
constant DOMAIN_ID : DOMAIN_ID_TYPE; -- Deferred to Package Body constant DOMAIN_ID : DOMAIN_ID_TYPE; -- Deferred to Package Body
type DOMAIN_ENDPOINT_MAP_TYPE is array (NUM_DOMAIN-1 downto 0) of std_logic_vector(NUM_READERS+NUM_WRITERS-1 downto 0); type DOMAIN_ENDPOINT_MAP_TYPE is array (NUM_DOMAIN-1 downto 0) of std_logic_vector(MAX_ENDPOINTS-1 downto 0);
constant DOMAIN_ENDPOINT_MAP : DOMAIN_ENDPOINT_MAP_TYPE; -- Deferred to Package Body constant DOMAIN_ENDPOINT_MAP : DOMAIN_ENDPOINT_MAP_TYPE; -- Deferred to Package Body
-- Since this implementation runs on the same network stack and the RTPS Endpoints (Readers & Writers) -- Since this implementation runs on the same network stack and the RTPS Endpoints (Readers & Writers)
@ -126,7 +131,7 @@ package rtps_package is
-- DDSI-RTPS 2.3 states that Entity IDs have to be unique within each Participant. -- DDSI-RTPS 2.3 states that Entity IDs have to be unique within each Participant.
-- For simplicity and ease of mapping we make the Entity IDs unique across all Participant and Domains on this node. -- For simplicity and ease of mapping we make the Entity IDs unique across all Participant and Domains on this node.
type ENTITYID_TYPE is array (NUM_READERS+NUM_WRITERS-1 downto 0) of std_logic_vector(ENTITYID_WIDTH-1 downto 0); type ENTITYID_TYPE is array (MAX_ENDPOINTS-1 downto 0) of std_logic_vector(ENTITYID_WIDTH-1 downto 0);
constant ENTITYID : ENTITYID_TYPE; -- Deferred to Package Body constant ENTITYID : ENTITYID_TYPE; -- Deferred to Package Body
constant ENTITYID_UNKNOWN : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0'); constant ENTITYID_UNKNOWN : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0');
constant ENTITYID_PARTICIPANT : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000001c1"); constant ENTITYID_PARTICIPANT : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000001c1");
@ -151,7 +156,7 @@ package rtps_package is
--**************** --****************
type USER_ENDPOINT_OUTPUT is array (NUM_READERS+NUM_WRITERS-1 downto 0) of std_logic_vector(31 downto 0); type USER_ENDPOINT_OUTPUT is array (MAX_ENDPOINTS-1 downto 0) of std_logic_vector(31 downto 0);
type BUILTIN_ENDPOINT_TYPE is array (NUM_DOMAIN-1 downto 0) of std_logic_vector(31 downto 0); type BUILTIN_ENDPOINT_TYPE is array (NUM_DOMAIN-1 downto 0) of std_logic_vector(31 downto 0);
end package; end package;
@ -178,7 +183,7 @@ package body rtps_package is
begin begin
ret := (others => (others => '0')); ret := (others => (others => '0'));
for i in 0 to NUM_DOMAIN-1 loop for i in 0 to NUM_DOMAIN-1 loop
for j in 0 to NUM_READERS+NUM_WRITERS-1 loop for j in 0 to MAX_ENDPOINTS-1 loop
if (i = end_id(j)) then if (i = end_id(j)) then
ret(i)(j) := '1'; ret(i)(j) := '1';
end if; end if;