300 lines
16 KiB
VHDL
300 lines
16 KiB
VHDL
library ieee;
|
|
use ieee.std_logic_1164.all;
|
|
use ieee.numeric_std.all;
|
|
|
|
package rtps_package is
|
|
|
|
--*****USER CONFIG*****
|
|
-- Unicast IPv4 Address used by all RTPS Entities [Default 192.168.0.80]
|
|
constant IPv4_UNICAST_ADDRESS : std_logic_vector(31 downto 0) := x"C0A80080";
|
|
-- Number of RTPS Writer Endpoints
|
|
constant NUM_WRITERS : integer := 0;
|
|
-- Number of RTPS Reader Endpoints
|
|
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)
|
|
constant PORT_CONFIG_PB : integer := 7400;
|
|
-- DG Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)
|
|
constant PORT_CONFIG_DG : integer := 250;
|
|
-- PG Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)
|
|
constant PORT_CONFIG_PG : integer := 2;
|
|
-- D0 Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)
|
|
constant PORT_CONFIG_D0 : integer := 0;
|
|
-- D1 Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)
|
|
constant PORT_CONFIG_D1 : integer := 10;
|
|
-- D2 Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)
|
|
constant PORT_CONFIG_D2 : integer := 1;
|
|
-- D3 Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)
|
|
constant PORT_CONFIG_D3 : integer := 11;
|
|
-- Number of Domains
|
|
constant NUM_DOMAIN : integer := 1;
|
|
-- MAC Address of underlying network stack (Used to generate GUIDs)
|
|
constant MAC_ADDRESS : std_logic_vector(47 downto 0) := x"97917E0BA8CF";
|
|
-----------------------------------------------------------------------------------------------------
|
|
-- *DO NOT MODIFY BEGIN*
|
|
type USER_DOMAIN_ID_TYPE is array (NUM_DOMAIN-1 downto 0) of integer;
|
|
-- *DO NOT MODIFY END*
|
|
-----------------------------------------------------------------------------------------------------
|
|
-- Array of Domain IDs
|
|
constant USER_DOMAIN_ID : USER_DOMAIN_ID_TYPE := (0 => 1);
|
|
|
|
-----------------------------------------------------------------------------------------------------
|
|
-- *DO NOT MODIFY BEGIN*
|
|
type ENDPOINT_DOMAIN_MAP_TYPE is array (MAX_ENDPOINTS-1 downto 0) of integer;
|
|
type ENDPOINT_WITH_KEY_TYPE is array (MAX_ENDPOINTS-1 downto 0) of boolean;
|
|
-- *DO NOT MODIFY END*
|
|
-----------------------------------------------------------------------------------------------------
|
|
|
|
--***RTPS ENDPOINTS***
|
|
-- Array mapping the RTPS Endpoints to their respective Domain IDs
|
|
-- The index of this array denotes the Endpoint, and the element value the index of the Domain in the 'USER_DOMAIN_ID'.
|
|
constant ENDPOINT_DOMAIN_MAP: ENDPOINT_DOMAIN_MAP_TYPE := (0 => 0);
|
|
-- Array denoting if Endpoints use Keyed Topics
|
|
constant ENDPOINT_WITH_KEY : ENDPOINT_WITH_KEY_TYPE := (0 => FALSE);
|
|
|
|
|
|
--*****DDSI-RTPS 2.3*****
|
|
|
|
-- Default Multicast Ipv4 Address (239.255.0.1)
|
|
constant DEFAULT_IPv4_MULTICAST_ADDRESS : std_logic_vector(31 downto 0) := x"EFFF0001";
|
|
|
|
constant GUIDPREFIX_WIDTH : integer := 96;
|
|
constant PROTOCOLVERSION_WIDTH : integer := 16;
|
|
constant VENDORID_WIDTH : integer := 16;
|
|
constant SUBMESSAGE_ID_WIDTH : integer := 8;
|
|
constant DOMAIN_ID_WIDTH : integer := 32;
|
|
constant UDP_PORT_WIDTH : integer := 16;
|
|
constant ENTITYID_WIDTH : integer := 32;
|
|
constant PROTOCOL_WIDTH : integer := 32;
|
|
|
|
-- 'RTPS' in Ascii code
|
|
constant PROTOCOL_RTPS : std_logic_vector(PROTOCOL_WIDTH-1 downto 0) := x"52545053";
|
|
constant PROTOCOLVERSION_2_4 : std_logic_vector(PROTOCOLVERSION_WIDTH-1 downto 0) := x"0204";
|
|
constant VENDORID_UNKNOWN : std_logic_vector(VENDORID_WIDTH-1 downto 0) := (others => '0');
|
|
constant VENDORID : std_logic_vector(VENDORID_WIDTH-1 downto 0) := VENDORID_UNKNOWN;
|
|
-- Submessage IDs
|
|
constant SID_PAD : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"01";
|
|
constant SID_ACKNACK : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"06";
|
|
constant SID_HEARTBEAT : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"07";
|
|
constant SID_GAP : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"08";
|
|
constant SID_INFO_TS : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"09";
|
|
constant SID_INFO_SRC : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"0c";
|
|
constant SID_INFO_REPLY_IP4 : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"0d";
|
|
constant SID_INFO_DST : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"0e";
|
|
constant SID_INFO_REPLY : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"0f";
|
|
constant SID_NACK_FRAG : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"12";
|
|
constant SID_HEARTBEAT_FRAG : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"13";
|
|
constant SID_DATA : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"15";
|
|
constant SID_DATA_FRAG : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"16";
|
|
|
|
|
|
|
|
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
|
|
|
|
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
|
|
|
|
-- Since this implementation runs on the same network stack and the RTPS Endpoints (Readers & Writers)
|
|
-- can be differentiated based on their Entity ID, it makes no sense to have multiple IP Addresses
|
|
-- (and hence multiple Participants).
|
|
-- We generate just single participant for every Domain, and later match the Endpoints to their respective
|
|
-- Domain (and thus also to their respective RTPS Participant).
|
|
|
|
type IPv4_PORT_TYPE is array (NUM_DOMAIN-1 downto 0) of std_logic_vector(UDP_PORT_WIDTH-1 downto 0);
|
|
constant META_IPv4_MULTICAST_PORT: IPv4_PORT_TYPE; -- Deferred to Package Body
|
|
constant META_IPv4_UNICAST_PORT : IPv4_PORT_TYPE; -- Deferred to Package Body
|
|
constant USER_IPv4_MULTICAST_PORT: IPv4_PORT_TYPE; -- Deferred to Package Body
|
|
constant USER_IPv4_UNICAST_PORT : IPv4_PORT_TYPE; -- Deferred to Package Body
|
|
|
|
type GUIDPREFIX_TYPE is array (NUM_DOMAIN-1 downto 0) of std_logic_vector(GUIDPREFIX_WIDTH-1 downto 0);
|
|
constant GUIDPREFIX : GUIDPREFIX_TYPE; -- Deferred to Package Body
|
|
constant GUIDPREFIX_UNKNOWN : std_logic_vector(GUIDPREFIX_WIDTH-1 downto 0) := (others => '0');
|
|
|
|
subtype ENTITY_KIND_H is std_logic_vector(1 downto 0);
|
|
subtype ENTITY_KIND_L is std_logic_vector(5 downto 0);
|
|
--FOLLOWING MAP TO ENTITY_KIND_H
|
|
constant USER_DEFINED_ENTITY : ENTITY_KIND_H := "00";
|
|
constant BUILT_IN_ENTITY : ENTITY_KIND_H := "11";
|
|
constant VENDOR_SPECIFIC_ENTITY : ENTITY_KIND_H := "01";
|
|
--FOLLOWING MAP TO ENTITY_KIND_L
|
|
constant WRITER_WITH_KEY : ENTITY_KIND_L := "000010";
|
|
constant WRITER_NO_KEY : ENTITY_KIND_L := "000011";
|
|
constant READER_NO_KEY : ENTITY_KIND_L := "000100";
|
|
constant READER_WITH_KEY : ENTITY_KIND_L := "000111";
|
|
constant WRITER_GROUP : ENTITY_KIND_L := "001000";
|
|
constant READER_GROUP : ENTITY_KIND_L := "001001";
|
|
|
|
-- 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.
|
|
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_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_SEDP_BUILTIN_TOPICS_ANNOUNCER : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000002c2");
|
|
constant ENTITYID_SEDP_BUILTIN_TOPICS_DETECTOR : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000002c7");
|
|
constant ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000003c2");
|
|
constant ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000003c7");
|
|
constant ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000004c2");
|
|
constant ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000004c7");
|
|
constant ENTITYID_SPDP_BUILTIN_PARTICIPANT_ANNOUNCER : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000100c2");
|
|
constant ENTITYID_SPDP_BUILTIN_PARTICIPANT_DETECTOR : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000100c7");
|
|
constant ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER: std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000200c2");
|
|
constant ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER: std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000200c7");
|
|
|
|
constant LOCATOR_KIND_WIDTH : integer := 32;
|
|
constant LOCATOR_KIND_INVALID : std_logic_vector := std_logic_vector(to_signed(-1,LOCATOR_KIND_WIDTH));
|
|
constant LOCATOR_KIND_RESERVERD : std_logic_vector := std_logic_vector(to_signed(0,LOCATOR_KIND_WIDTH));
|
|
constant LOCATOR_KIND_UDPv4 : std_logic_vector := std_logic_vector(to_signed(1,LOCATOR_KIND_WIDTH));
|
|
constant LOCATOR_KIND_UDPv6 : std_logic_vector := std_logic_vector(to_signed(2,LOCATOR_KIND_WIDTH));
|
|
|
|
--*****CUSTOM*****
|
|
|
|
--****************
|
|
|
|
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);
|
|
|
|
end package;
|
|
|
|
package body rtps_package is
|
|
|
|
function gen_domain_ids (user_id : USER_DOMAIN_ID_TYPE) return DOMAIN_ID_TYPE is
|
|
variable ret : DOMAIN_ID_TYPE;
|
|
begin
|
|
ret := (others => (others => '0'));
|
|
for i in 0 to user_id'length-1 loop
|
|
-- Check if User provided Domain ID fits
|
|
-- NOTE: Cannot assert due to vhdl integer overflow.
|
|
--assert (user_id(i) < (2**DOMAIN_ID_WIDTH-1 - PORT_CONFIG_PB) / PORT_CONFIG_DG) report "Domain ID range exceeded" severity failure;
|
|
ret(i) := std_logic_vector(to_unsigned(user_id(i), ret(i)'length));
|
|
end loop;
|
|
return ret;
|
|
end function;
|
|
|
|
constant DOMAIN_ID : DOMAIN_ID_TYPE := gen_domain_ids(USER_DOMAIN_ID);
|
|
|
|
function gen_domain_endpoint_map (end_id : ENDPOINT_DOMAIN_MAP_TYPE) return DOMAIN_ENDPOINT_MAP_TYPE is
|
|
variable ret : DOMAIN_ENDPOINT_MAP_TYPE;
|
|
begin
|
|
ret := (others => (others => '0'));
|
|
for i in 0 to NUM_DOMAIN-1 loop
|
|
for j in 0 to MAX_ENDPOINTS-1 loop
|
|
if (i = end_id(j)) then
|
|
ret(i)(j) := '1';
|
|
end if;
|
|
end loop;
|
|
end loop;
|
|
return ret;
|
|
end function;
|
|
|
|
constant DOMAIN_ENDPOINT_MAP : DOMAIN_ENDPOINT_MAP_TYPE := gen_domain_endpoint_map(ENDPOINT_DOMAIN_MAP);
|
|
|
|
function gen_meta_multicast_ports (domain_id : DOMAIN_ID_TYPE) return IPv4_PORT_TYPE is
|
|
variable ret : IPv4_PORT_TYPE;
|
|
begin
|
|
ret := (others => (others => '0'));
|
|
for i in 0 to domain_id'length-1 loop
|
|
-- (see DDSI-RTPS 2.3 Section 9.6.1)
|
|
-- PB + DG * domain_id + d0
|
|
ret(i) := std_logic_vector(to_unsigned(PORT_CONFIG_PB + PORT_CONFIG_D0 + PORT_CONFIG_DG*to_integer(unsigned(domain_id(i))), ret(i)'length));
|
|
end loop;
|
|
return ret;
|
|
end function;
|
|
|
|
function gen_meta_unicast_ports (domain_id : DOMAIN_ID_TYPE) return IPv4_PORT_TYPE is
|
|
variable ret : IPv4_PORT_TYPE;
|
|
begin
|
|
ret := (others => (others => '0'));
|
|
for i in 0 to domain_id'length-1 loop
|
|
-- (see DDSI-RTPS 2.3 Section 9.6.1)
|
|
-- PB + DG * domainId + d1 + PG * participant_id
|
|
-- participant_id=0
|
|
ret(i) := std_logic_vector(to_unsigned(PORT_CONFIG_PB + PORT_CONFIG_D1 + PORT_CONFIG_DG*to_integer(unsigned(domain_id(i))), ret(i)'length));
|
|
end loop;
|
|
return ret;
|
|
end function;
|
|
|
|
function gen_user_multicast_ports (domain_id : DOMAIN_ID_TYPE) return IPv4_PORT_TYPE is
|
|
variable ret : IPv4_PORT_TYPE;
|
|
begin
|
|
ret := (others => (others => '0'));
|
|
for i in 0 to domain_id'length-1 loop
|
|
-- (see DDSI-RTPS 2.3 Section 9.6.1)
|
|
-- PB + DG * domainId + d2
|
|
ret(i) := std_logic_vector(to_unsigned(PORT_CONFIG_PB + PORT_CONFIG_D2 + PORT_CONFIG_DG*to_integer(unsigned(domain_id(i))), ret(i)'length));
|
|
end loop;
|
|
return ret;
|
|
end function;
|
|
|
|
function gen_user_unicast_ports (domain_id : DOMAIN_ID_TYPE) return IPv4_PORT_TYPE is
|
|
variable ret : IPv4_PORT_TYPE;
|
|
begin
|
|
ret := (others => (others => '0'));
|
|
for i in 0 to domain_id'length-1 loop
|
|
-- (see DDSI-RTPS 2.3 Section 9.6.1)
|
|
-- PB + DG * domainId + d3 + PG * participant_id
|
|
-- participant_id=0
|
|
ret(i) := std_logic_vector(to_unsigned(PORT_CONFIG_PB + PORT_CONFIG_D3 + PORT_CONFIG_DG*to_integer(unsigned(domain_id(i))), ret(i)'length));
|
|
end loop;
|
|
return ret;
|
|
end function;
|
|
|
|
constant META_IPv4_MULTICAST_PORT: IPv4_PORT_TYPE := gen_meta_multicast_ports(DOMAIN_ID);
|
|
constant META_IPv4_UNICAST_PORT : IPv4_PORT_TYPE := gen_meta_unicast_ports(DOMAIN_ID);
|
|
constant USER_IPv4_MULTICAST_PORT: IPv4_PORT_TYPE := gen_user_multicast_ports(DOMAIN_ID);
|
|
constant USER_IPv4_UNICAST_PORT : IPv4_PORT_TYPE := gen_user_unicast_ports(DOMAIN_ID);
|
|
|
|
function gen_guid_prefix (domain_id : DOMAIN_ID_TYPE) return GUIDPREFIX_TYPE is
|
|
variable ret : GUIDPREFIX_TYPE;
|
|
begin
|
|
ret := (others => (others => '0'));
|
|
for i in 0 to ret'length-1 loop
|
|
-- First to bytes have to be Vendor ID (see DDSI-RTPS 2.3 Section 9.3.1.5)
|
|
ret(i)(ret(i)'length-1 downto ret(i)'length-VENDORID_WIDTH) := VENDORID;
|
|
-- Next we insert the MAC address for uniqueness
|
|
ret(i)(ret(i)'length-VENDORID_WIDTH-1 downto ret(i)'length-VENDORID_WIDTH-48) := MAC_ADDRESS;
|
|
-- Next we insert the Domain ID
|
|
-- NOTE: If the widths of signals change in the future, this will overwrite part of the MAC Address.
|
|
ret(i)(DOMAIN_ID_WIDTH-1 downto 0) := domain_id(i);
|
|
end loop;
|
|
return ret;
|
|
end function;
|
|
|
|
constant GUIDPREFIX : GUIDPREFIX_TYPE := gen_guid_prefix(DOMAIN_ID);
|
|
|
|
function gen_entyid return ENTITYID_TYPE is
|
|
variable ret : ENTITYID_TYPE;
|
|
begin
|
|
ret := (others => (others => '0'));
|
|
for i in 0 to ret'length-1 loop
|
|
-- (see DDSI-RTPS 2.3 Section 9.3.1.2)
|
|
-- Entity Kind Mapping
|
|
ret(i)(7 downto 6) := USER_DEFINED_ENTITY;
|
|
if (i <= NUM_READERS-1) then
|
|
if (ENDPOINT_WITH_KEY(i)) then
|
|
ret(i)(5 downto 0) := READER_WITH_KEY;
|
|
else
|
|
ret(i)(5 downto 0) := READER_NO_KEY;
|
|
end if;
|
|
else
|
|
if (ENDPOINT_WITH_KEY(i)) then
|
|
ret(i)(5 downto 0) := WRITER_WITH_KEY;
|
|
else
|
|
ret(i)(5 downto 0) := WRITER_NO_KEY;
|
|
end if;
|
|
end if;
|
|
-- ID Mapping
|
|
ret(i)(ENTITYID_WIDTH-1 downto 8) := std_logic_vector(to_unsigned(i,ENTITYID_WIDTH-8));
|
|
end loop;
|
|
return ret;
|
|
end function;
|
|
|
|
constant ENTITYID : ENTITYID_TYPE := gen_entyid;
|
|
|
|
end package body;
|