diff --git a/Report.txt b/Report.txt new file mode 100644 index 0000000..757d1f0 --- /dev/null +++ b/Report.txt @@ -0,0 +1,127 @@ +--------------------------------------------------------------------------------- +Start RTL Component Statistics (1 Buffer) +--------------------------------------------------------------------------------- +Detailed RTL Component Info : ++---Adders : + 3 Input 14 Bit Adders := 1 + 2 Input 14 Bit Adders := 3 + 2 Input 9 Bit Adders := 1 ++---Registers : + 400 Bit Registers := 1 + 88 Bit Registers := 1 + 32 Bit Registers := 1 + 14 Bit Registers := 5 + 9 Bit Registers := 1 + 4 Bit Registers := 1 + 1 Bit Registers := 3 ++---RAMs : + 512K Bit RAMs := 1 ++---Muxes : + 2 Input 400 Bit Muxes := 2 + 18 Input 400 Bit Muxes := 1 + 2 Input 32 Bit Muxes := 10 + 18 Input 32 Bit Muxes := 2 + 2 Input 14 Bit Muxes := 1 + 18 Input 14 Bit Muxes := 4 + 18 Input 9 Bit Muxes := 1 + 17 Input 5 Bit Muxes := 1 + 2 Input 5 Bit Muxes := 10 + 2 Input 2 Bit Muxes := 2 + 2 Input 1 Bit Muxes := 7 + 18 Input 1 Bit Muxes := 16 +--------------------------------------------------------------------------------------------------- + State | New Encoding | Previous Encoding +--------------------------------------------------------------------------------------------------- + ipv4_init | 00000 | 00000 + ipv4_header_1 | 00001 | 00001 + ipv4_header_2 | 00010 | 00010 + ipv4_header_3 | 00011 | 00011 + ipv4_header_4 | 00100 | 00100 + ipv4_header_5 | 00101 | 00101 + skip_header | 00110 | 01111 + ipv4_fragment_pre | 00111 | 01000 + ipv4_fragment | 01000 | 01001 + ipv4_fragment_post | 01001 | 01010 + ipv4_buffer_src | 01010 | 01011 + ipv4_buffer_dest | 01011 | 01100 + ipv4_buffer_length | 01100 | 01101 + ipv4_buffer_payload | 01101 | 01110 + skip_packet | 01110 | 10000 + iSTATE | 01111 | 11111 + ipv4_payload_length | 10000 | 00110 + ipv4_payload | 10001 | 00111 +--------------------------------------------------------------------------------------------------- + +--------------------------------------------------------------------------------- +Start RTL Component Statistics (0 Buffer) +--------------------------------------------------------------------------------- +Detailed RTL Component Info : ++---Adders : + 2 Input 14 Bit Adders := 3 + 3 Input 14 Bit Adders := 1 + 2 Input 9 Bit Adders := 1 ++---Registers : + 400 Bit Registers := 1 + 88 Bit Registers := 1 + 14 Bit Registers := 5 + 9 Bit Registers := 1 + 4 Bit Registers := 1 + 1 Bit Registers := 2 ++---Muxes : + 2 Input 400 Bit Muxes := 2 + 2 Input 32 Bit Muxes := 7 + 12 Input 32 Bit Muxes := 1 + 2 Input 14 Bit Muxes := 1 + 11 Input 4 Bit Muxes := 1 + 2 Input 4 Bit Muxes := 7 + 2 Input 1 Bit Muxes := 4 + 12 Input 1 Bit Muxes := 7 +--------------------------------------------------------------------------------------------------- + State | New Encoding | Previous Encoding +--------------------------------------------------------------------------------------------------- + ipv4_init | 0000 | 00000 + ipv4_header_1 | 0001 | 00001 + ipv4_header_2 | 0010 | 00010 + ipv4_header_3 | 0011 | 00011 + ipv4_header_4 | 0100 | 00100 + ipv4_header_5 | 0101 | 00101 + skip_header | 0110 | 01111 + ipv4_fragment_pre | 0111 | 01000 + ipv4_payload_length | 1000 | 00110 + ipv4_payload | 1001 | 00111 + skip_packet | 1010 | 10000 + iSTATE | 1011 | 11111 +--------------------------------------------------------------------------------------------------- + +--------------------------------------------------------------------------------- +Start RTL Component Statistics (No Frag) +--------------------------------------------------------------------------------- +Detailed RTL Component Info : ++---Adders : + 2 Input 14 Bit Adders := 1 + 3 Input 14 Bit Adders := 1 ++---Registers : + 14 Bit Registers := 1 + 4 Bit Registers := 1 ++---Muxes : + 2 Input 32 Bit Muxes := 3 + 11 Input 32 Bit Muxes := 1 + 10 Input 4 Bit Muxes := 1 + 2 Input 4 Bit Muxes := 5 + 2 Input 1 Bit Muxes := 1 + 11 Input 1 Bit Muxes := 6 +--------------------------------------------------------------------------------------------------- + State | New Encoding | Previous Encoding +--------------------------------------------------------------------------------------------------- + ipv4_init | 0000 | 0000 + ipv4_header_1 | 0001 | 0001 + ipv4_header_2 | 0010 | 0010 + ipv4_header_3 | 0011 | 0011 + ipv4_header_4 | 0100 | 0100 + ipv4_header_5 | 0101 | 0101 + skip_header | 0110 | 1000 + ipv4_payload_length | 0111 | 0110 + ipv4_payload | 1000 | 0111 + skip_packet | 1001 | 1001 + iSTATE | 1010 | 1111 +--------------------------------------------------------------------------------------------------- \ No newline at end of file diff --git a/doc/rfc768.pdf b/doc/rfc768.pdf new file mode 100644 index 0000000..0a6a47d --- /dev/null +++ b/doc/rfc768.pdf @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5da10961c599f0f0678828948c1bcbbef1065a9f7f7a46293d9ca19aad300597 +size 9055 diff --git a/src/TODO.txt b/src/TODO.txt new file mode 100644 index 0000000..d1f4f2a --- /dev/null +++ b/src/TODO.txt @@ -0,0 +1,87 @@ +RULES 8.4.2 +=========== + +GENERAL +------- +* All communications must take place using RTPS Messages +* All implementations must implement the RTPS Message Receiver +* The timing characteristics of all implementations must be tunable +* Implementations must implement the Simple Participant and Endpoint Discovery Protocols + +WRITER +------ +* Writers must not send data out-of-order +* Writers must include in-line QoS values if requested by a Reader +* Writers must send periodic HEARTBEAT Messages (reliable only) +* Writers must eventually respond to a negative acknowledgment (reliable only) +* Sending Heartbeats and Gaps with Writer Group Information (Writer belonging to a Group) + +READER +------ +A best-effort Reader is completely passive as it only receives data and does not send messages itself. +Therefore, the requirements below only apply to reliable Readers. +* Readers must respond eventually after receiving a HEARTBEAT with final flag not set +* Readers must respond eventually after receiving a HEARTBEAT that indicates a sample is missing +* Once acknowledged, always acknowledged +* Readers can only send an ACKNACK Message in response to a HEARTBEAT Message + +RELIABILITY +=========== +* Best Effort Writer can only be matched with Best Effort Reader +* Stateless Reader can only be Best Effort (maintains absolutely no state, does not handle duplicate and out-of-order changes) + +STATELESS WRITER +================ +Note that the processing of this message uses the reply locators in the RTPS Receiver. +This is the only source of information for the StatelessWriter to determine where to send the reply to. +Proper functioning of the protocol requires that the RTPS Reader inserts an InfoReply Submessage ahead of the AckNack such that these fields are properly set. + +Writer Liveness Protocol +======================== +ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER +ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER + +OPTIONAL 8.4.14 +=============== +Optional features may not be supported by all RTPS implementations. + +* LARGE DATA (Fragmented Data) + +-------------------------------------------- + +ENTITYID_UKNOWN also for Built-In? +Ignore Participant/Topic/Publication/Subscription (handle argument of Sampleinfo) + +ENDIANNESS +========== +You have to see what datatypes PSM maps to each element. +If the datatype is bigger than a byte, byte swaping has to occur. +The elements of an array are in order (but the elements themselves may need to be swapped if bigger than a Byte) + +ENDPOINT FIFO PACKET FORMAT +=========================== + +0...2...........8...............16..............24..............32 ++-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ +| LENTGH | ++---------------+---------------+---------------+---------------+ +| OPCODE | FLAGS | UDP_PORT | ++---------------+---------------+---------------+---------------+ +| IPv4_ADDR | ++---------------------------------------------------------------+ +| ENTITYID | ++---------------------------------------------------------------+ +| | ++ Sequence Number [only for DATA Submessage] + +| | ++---------------------------------------------------------------+ +| | +~ PAYLOAD (SUBMESSAGE CONTENT) ~ +| | ++---------------------------------------------------------------+ + +ENDPOINT_ID +=========== + +MSB...........LSB +READERS...WRITERS \ No newline at end of file diff --git a/src/addsub.vhd b/src/addsub.vhd new file mode 100644 index 0000000..0c7595a --- /dev/null +++ b/src/addsub.vhd @@ -0,0 +1,76 @@ +library ieee; +use ieee.std_logic_1164.all; +use ieee.numeric_std.all; + +Library UNISIM; +use UNISIM.vcomponents.all; + +Library UNIMACRO; +use UNIMACRO.vcomponents.all; + +-- Add/Sub +-- This entity adds or subtracts inputs 'A' and 'B', depending on 'mode' (1 = add, 0 = sub). +-- If 'cap' is high, on Overfolw/Underflow conditions the result is capped at max/min value. + +entity addsub is + generic ( + PIPELINE_STAGES : integer := 1; + DATA_WIDTH : integer := 16 + ); + port ( + clk : in std_logic; + reset : in std_logic; + mode : in std_logic; + cap : in std_logic; + A : in std_logic_vector(DATA_WIDTH-1 downto 0); + B : in std_logic_vector(DATA_WIDTH-1 downto 0); + RES : out std_logic_vector(DATA_WIDTH-1 downto 0) + ); +end entity; + +architecture arch of addsub is + + --*****SIGNAl DECLARATION + signal result : std_logic_vector(DATA_WIDTH-1 downto 0) := (others => '0'); + signal carry : std_logic := '0'; + +begin + + ADDSUB_MACRO_inst : ADDSUB_MACRO + generic map ( + DEVICE => "7SERIES", -- Target Device: "VIRTEX5", "7SERIES", "SPARTAN6" + LATENCY => PIPELINE_STAGES, -- Desired clock cycle latency, 0-2 + WIDTH => DATA_WIDTH -- Input / Output bus width, 1-48 + ) + port map ( + CARRYOUT => open, -- 1-bit carry-out output signal + RESULT => result, -- Add/sub result output, width defined by WIDTH generic + A => A, -- Input A bus, width defined by WIDTH generic + ADD_SUB => mode, -- 1-bit add/sub input, high selects add, low selects subtract + B => B, -- Input B bus, width defined by WIDTH generic + CARRYIN => '0', -- 1-bit carry-in input + CE => '1', -- 1-bit clock enable input + CLK => clk, -- 1-bit clock input + RST => reset -- 1-bit active high synchronous reset + ); + + clamp : process(all) + begin + --DEFAULT VALUE + RES <= result; + + --Overflow/Underflow + if(carry = '1' and cap = '1') then + --ADD + if(mode = '1') then + --CAP AT MAX VALUE + RES <= (others => '1'); + --SUB + else + --CAP AT ZERO + RES <= (others => '0'); + end if; + end if; + end process; + +end architecture; \ No newline at end of file diff --git a/src/checksum.vhd b/src/checksum.vhd new file mode 100644 index 0000000..c5130e3 --- /dev/null +++ b/src/checksum.vhd @@ -0,0 +1,47 @@ +library ieee; +use ieee.std_logic_1164.all; +use ieee.numeric_std.all; + + +entity addsub is + generic ( + INPUT_WIDTH : integer := 32; + OUTPUT_WIDTH : integer := 16 + ); + port ( + clk : in std_logic; + reset : in std_logic; + op : in std_logic_vector(1 downto 0); + input : in std_logic_vector(INPUT_WIDTH-1 downto 0); + output : out std_logic_vector(OUTPUT_WIDTH-1 downto 0); + done : out std_logic + ); +end entity; + +architecture arch of addsub is + + --*****COMPONENT DECLARATION***** + entity addsub is + generic ( + PIPELINE_STAGES : integer := 1; + DATA_WIDTH : integer := 32 + ); + port ( + clk : in std_logic; + reset : in std_logic; + cin : in std_logic; + A : in std_logic_vector(DATA_WIDTH-1 downto 0); + B : in std_logic_vector(DATA_WIDTH-1 downto 0); + RES : out std_logic_vector(DATA_WIDTH-1 downto 0); + cout : out std_logic + ); + end entity; + + --*****SIGNAl DECLARATION + signal result : std_logic_vector(DATA_WIDTH-1 downto 0) := (others => '0'); + signal carry : std_logic := '0'; + +begin + + +end architecture; \ No newline at end of file diff --git a/src/ipv4_in.vhd b/src/ipv4_in_handler.vhd similarity index 64% rename from src/ipv4_in.vhd rename to src/ipv4_in_handler.vhd index ea23657..827979b 100644 --- a/src/ipv4_in.vhd +++ b/src/ipv4_in_handler.vhd @@ -5,26 +5,76 @@ use ieee.numeric_std.all; use work.ip_package.all; use work.math_pkg.all; -entity ipv4_in is +-- IPv4 Handler +-- This entity parses IPv4 packets read from the input FIFO, and extracts the underlying Protocol (Payload) +-- to the respective output FIFO together with the Src and Dest Addresses. + +-- The input FIFO has to be a 32-bit wide FWFT (First Word Fall Through) FIFO. The IPv4 packet has to be +-- 32-bit word aligned (padded with zeroes in the end to the closest 32-bit boundary), and the first word +-- before the actual packet has to be the length of the following IPv4 packet (in 32-bit words + padding). +-- This is necessary to recover from spurious packets. +-- ex. +-- 31 0 +-- +------------------------------+ +-- | LENGTH=4 | +-- +------------------------------+ +-- | IPv4 Packet | +-- + + +-- | | +-- + + +-- | | +-- + +--------------+ +-- | | PADDING | +-- +---------------+--------------+ + +-- The output FIFO distribution is controlled by the 'LAYER3_PROTOCOLS' constant in the 'ip_package' package. +-- The array position of the protocol version number is also the id of the output FIFO where this protocol +-- is sent to. First word on the output FIFO is the Source address, second the Destination Address, third +-- the payload length (in 32-bit words + padding), followed by the actual packet. +-- ex. +-- 31 0 +-- +------------------------------+ +-- | Source Address | +-- +------------------------------+ +-- | Destination Address | +-- +------------------------------+ +-- | LENGTH=4 | +-- +------------------------------+ +-- | UDP Packet | +-- + + +-- | | +-- | + +-- | | +-- + +--------------+ +-- | | PADDING | +-- +---------------+--------------+ + +-- This Entity features 2 architecture implementations. +-- The 'no_frag' implementation does not support fragmentation and just drops fragmented packets. This needs the least amount of resources +-- The 'with_frag' implementation supports fragmentation as stated in RFC 791 (fragments of any size received in any order with auto adjusted +-- timeout according to transmitted TTL). The re-assembly needs considerable more resources for packet buffering and state holding. +-- The generics allow tuning of the supported packet sizes and used resources. + +entity ipv4_in_handler is generic( - CLK_FREQ : integer := 20000000; - MAX_FRAG_SIZE : integer := 1600; - MAX_PARALLEL_FRAG : integer := 1; - DEFAULT_FRAG_TIMEOUT: integer := 5 + CLK_FREQ : integer := 20000000; -- Frequency of input clock in Hz + MAX_FRAG_SIZE : integer := 1600; -- Maximum re-assembled IP packet size. If a re-assembled packet exceeds this threshold it is dropped. (Does not limit the size of received non-fragmented packets) + MAX_PARALLEL_FRAG : integer := 1; -- Maximum number of parallel running re-assemble procedures. If a fragment of a new Ip packet is received, while 'MAX_PARALLEL_FRAG' packets are already in the re-assembly procedure, it is dropped. + DEFAULT_FRAG_TIMEOUT: integer := 5 -- Default timeout in seconds until which the re-assembly procedure of an IP packet has to be completed before being dropped to fre up resources. ); port ( - clk : in std_logic; - reset : in std_logic; - empty : in std_logic; - data_in : in std_logic_vector(31 downto 0); - rd : out std_logic; - wr : out std_logic_vector(LAYER3_PROTOCOL_NUM-1 downto 0); - full : in std_logic_vector(LAYER3_PROTOCOL_NUM-1 downto 0); - data_out: out IP_OUTPUT_TYPE + clk : in std_logic; -- Input Clock + reset : in std_logic; -- Synchronous Reset + empty : in std_logic; -- Input FIFO empty flag + rd : out std_logic; -- Input FIFO read signal + data_in : in std_logic_vector(31 downto 0); -- Input FIFO data signal + full : in std_logic_vector(LAYER3_PROTOCOL_NUM-1 downto 0); -- Output FIFO full flag + wr : out std_logic_vector(LAYER3_PROTOCOL_NUM-1 downto 0); -- Output FIFO write signal + data_out: out IP_OUTPUT_TYPE -- Output FIFO data signal ); end entity; -architecture arch of ipv4_in is +architecture with_frag of ipv4_in_handler is --*****COMPONENT DECLARATION***** -- The RAM is used for reassembly of ip fragments. @@ -60,15 +110,15 @@ architecture arch of ipv4_in is -- NOTE: The MAX_PARALLEL_FRAG array position is not mapped to a buffer, but is used for comparison purposes type BUFFER_ID_ARRAY is array (MAX_PARALLEL_FRAG downto 0) of std_logic_vector(87 downto 0); -- Array of bitmaps used to identify received fragment words. - type BUFFER_BITMAP_ARRAY is array (max(MAX_PARALLEL_FRAG-1,0) downto 0) of std_logic_vector((MAX_FRAG_SIZE/4)-1 downto 0); + type BUFFER_BITMAP_ARRAY is array (MAX_PARALLEL_FRAG-1 downto 0) of std_logic_vector((MAX_FRAG_SIZE/4)-1 downto 0); -- Array of buffer timers used for timeout checking - type BUFFER_TIMER_ARRAY is array (max(MAX_PARALLEL_FRAG-1,0) downto 0) of unsigned(7 downto 0); + type BUFFER_TIMER_ARRAY is array (MAX_PARALLEL_FRAG-1 downto 0) of unsigned(7 downto 0); -- Array of address offsets for the RAM. Used as constant - type BUFFER_ADDR_OFFSET_TYPE is array (max(MAX_PARALLEL_FRAG-1,0) downto 0) of unsigned(RAM_ADDR_WIDTH-1 downto 0); + type BUFFER_ADDR_OFFSET_TYPE is array (MAX_PARALLEL_FRAG-1 downto 0) of unsigned(RAM_ADDR_WIDTH-1 downto 0); -- Array of packet sizes in buffers - type FRAG_SIZE_ARRAY is array (max(MAX_PARALLEL_FRAG-1,0) downto 0) of std_logic_vector(13 downto 0); + type FRAG_SIZE_ARRAY is array (MAX_PARALLEL_FRAG-1 downto 0) of std_logic_vector(13 downto 0); -- Array of 32-bit buffer word counters. Used for counting new received fragments and determining if the packet is completely re-assebled - type BUFFER_WORD_COUNTER_ARRAY is array (max(MAX_PARALLEL_FRAG-1,0) downto 0) of integer range 0 to (MAX_FRAG_SIZE/4); + type BUFFER_WORD_COUNTER_ARRAY is array (MAX_PARALLEL_FRAG-1 downto 0) of integer range 0 to (MAX_FRAG_SIZE/4); --*****SIGNAL DECLARATION***** @@ -113,9 +163,9 @@ architecture arch of ipv4_in is -- Signal used to reset buffer timers signal reset_buffer_timer : std_logic := '0'; -- ID of buffer timer to be reset - signal reset_buffer_timer_id : integer range 0 to max(MAX_PARALLEL_FRAG-1,0) := 0; + signal reset_buffer_timer_id : integer range 0 to MAX_PARALLEL_FRAG-1 := 0; -- ID of current buffer - signal cur_buffer_id, cur_buffer_id_next : integer range 0 to max(MAX_PARALLEL_FRAG-1,0) := 0; + signal cur_buffer_id, cur_buffer_id_next : integer range 0 to MAX_PARALLEL_FRAG-1 := 0; -- Signal used to reset the word counter signal reset_read_cnt : std_logic; -- Intermediate output signal @@ -191,37 +241,37 @@ architecture arch of ipv4_in is begin -- Generic Assertions - assert (MAX_PARALLEL_FRAG >= 0) report "MAX_PARALLEL_FRAG has to be positive" severity failure; + assert (MAX_PARALLEL_FRAG > 0) report "MAX_PARALLEL_FRAG has to be greater than 0" severity failure; assert (MAX_FRAG_SIZE mod 4 = 0) report "MAX_FRAG_SIZE has to be multiple of 4" severity failure; assert (MAX_FRAG_SIZE <= 65516) report "MAX_FRAG_SIZE has to be smaller or equal to 65516" severity error; assert (DEFAULT_FRAG_TIMEOUT <= 255) report "DEFAULT_FRAG_TIMEOUT has to be smaller or equal to 255" severity error; -- Generic Dependant Constant -- Does not change its value after generation + -- TODO: Change to constant with function initialization const_gen : for i in 0 to buffer_addr_offset'length-1 generate buffer_addr_offset(i) <= to_unsigned(MAX_FRAG_SIZE*i,buffer_addr_offset(i)'length); end generate; --*****COMPONENT INSTANTIATION***** - buffer_gen : if (MAX_PARALLEL_FRAG /= 0) generate - ram_inst : single_port_ram - generic map ( - ADDR_WIDTH => RAM_ADDR_WIDTH, - DATA_WIDTH => 32, - MEMORY_SIZE => MAX_PARALLEL_FRAG*MAX_FRAG_SIZE - ) - port map ( - clk => clk, - addr => std_logic_vector(buffer_addr), - wen => buffer_wen, - ren => buffer_ren, - wr_data => buffer_wr_data, - rd_data => buffer_rd_data - ); - end generate; + ram_inst : single_port_ram + generic map ( + ADDR_WIDTH => RAM_ADDR_WIDTH, + DATA_WIDTH => 32, + MEMORY_SIZE => MAX_PARALLEL_FRAG*MAX_FRAG_SIZE + ) + port map ( + clk => clk, + addr => std_logic_vector(buffer_addr), + wen => buffer_wen, + ren => buffer_ren, + wr_data => buffer_wr_data, + rd_data => buffer_rd_data + ); rd <= rd_sig; + -- TODO: Make only wr signal conditional and connect ALL output FIFOs to the Signal -- This process is responsible for MUXing the correct signal to the correct output FIFO output_prc : process(all) begin @@ -294,7 +344,7 @@ begin -- Reset packet Byte Counter reset_read_cnt <= '1'; -- Check Buffer timers for timeouts - for i in 0 to max(MAX_PARALLEL_FRAG-1,0) loop + for i in 0 to MAX_PARALLEL_FRAG-1 loop -- If timeout reached if(buffer_timer(i) >= max_buffer_timer(i)) then -- Free Buffer ID @@ -334,22 +384,16 @@ begin stage_next <= IPv4_HEADER_3; -- IP Fragmentation else - -- Fragmentation Reassembly disabled, skip packet - if (MAX_PARALLEL_FRAG = 0) then - stage_next <= SKIP_PACKET; - -- Store Fragment relevant data - else - is_fragment_next <= '1'; - frag_offset_next <= unsigned(std_logic_vector'(ip_frag_offset & "0")); --'std_logic_vector needed to avoid "Ambiguous type in infix expression" - -- Temporal storage for comparison - buffer_id_next(MAX_PARALLEL_FRAG)(15 downto 0) <= ip_id; - -- Needed to determine the total size of the reassembled packet - if (ip_MF_flag = '0') then - is_last_fragment_next <= '1'; - end if; - -- Continue Parsing - stage_next <= IPv4_HEADER_3; + is_fragment_next <= '1'; + frag_offset_next <= unsigned(std_logic_vector'(ip_frag_offset & "0")); --'std_logic_vector needed to avoid "Ambiguous type in infix expression" + -- Temporal storage for comparison + buffer_id_next(MAX_PARALLEL_FRAG)(15 downto 0) <= ip_id; + -- Needed to determine the total size of the reassembled packet + if (ip_MF_flag = '0') then + is_last_fragment_next <= '1'; end if; + -- Continue Parsing + stage_next <= IPv4_HEADER_3; end if; end if; -- Third IPv4 Header word (Fields: TTL, Protocol, Checksum) @@ -642,7 +686,7 @@ begin buffer_timer(reset_buffer_timer_id) <= (others => '0'); else -- Increment Timers - for i in 0 to max(MAX_PARALLEL_FRAG-1,0) loop + for i in 0 to MAX_PARALLEL_FRAG-1 loop -- Timers stay at highest value until reset if(buffer_timer(i) /= (buffer_timer(i)'reverse_range => '1')) then buffer_timer(i) <= buffer_timer(i) + 1; @@ -726,4 +770,267 @@ begin end if; end process; +end architecture; + +architecture no_frag of ipv4_in_handler is + + + --*****TYPE DECLARATION***** + -- FSM states. Explained delow in detail + type PARSER_STAGE_TYPE is (IPv4_INIT, IPv4_HEADER_1, IPv4_HEADER_2, IPv4_HEADER_3, IPv4_HEADER_4, + IPv4_HEADER_5, IPv4_PAYLOAD_LENGTH, IPv4_PAYLOAD, SKIP_HEADER, SKIP_PACKET); + + --*****SIGNAL DECLARATION***** + -- FSM state + signal stage, stage_next : PARSER_STAGE_TYPE := IPv4_INIT; + -- 32-bit aligned total packet length + signal packet_length, packet_length_next : unsigned(13 downto 0) := (others => '0'); + -- 32-bit aligned header length + signal header_length, header_length_next : unsigned(3 downto 0) := (others => '0'); + -- 32-bit word counter (Counts words read from input fifo) + signal read_cnt : unsigned(13 downto 0) := (others => '0'); + -- Intermediate input read signal. (Read from output port not allowed) + signal rd_sig : std_logic := '0'; + -- ID of current output fifo (Used to MUX output FIFOs) + signal output_id, output_id_next : integer range 0 to LAYER3_PROTOCOL_NUM-1 := 0; + -- Signal used to reset the word counter + signal reset_read_cnt : std_logic; + -- Intermediate output signal + signal data_out_sig : std_logic_vector(31 downto 0); + -- Intermediate output signal, and its 1 clk cycle delayed variant + signal wr_sig : std_logic; + + --*****ALIAS DEFINATION***** + --IPv4 HEADER + alias ip_version : std_logic_vector(3 downto 0) is data_in(31 downto 28); + alias ip_ihl : std_logic_vector(3 downto 0) is data_in(27 downto 24); + alias ip_dscp : std_logic_vector(5 downto 0) is data_in(23 downto 18); + alias ip_ecn : std_logic_vector(1 downto 0) is data_in(17 downto 16); + alias ip_length : std_logic_vector(15 downto 0) is data_in(15 downto 0); + alias ip_id : std_logic_vector(15 downto 0) is data_in(31 downto 16); + alias ip_DF_flag : std_logic is data_in(14); + alias ip_MF_flag : std_logic is data_in(13); + alias ip_frag_offset: std_logic_vector(12 downto 0) is data_in(12 downto 0); + alias ip_ttl : std_logic_vector(7 downto 0) is data_in(31 downto 24); + alias ip_protocol : std_logic_vector(7 downto 0) is data_in(23 downto 16); + alias ip_checksum : std_logic_vector(15 downto 0) is data_in(15 downto 0); + + --*****FUNCTION DECLARATION***** + + -- Truncates the lower 2 bits of the input, and if they are not equal zero, adds one to the result. + -- This is used to round the byte length to 32-bit word length. + function normalize_length (len : std_logic_vector(15 downto 0)) return std_logic_vector is + variable tmp : std_logic_vector(13 downto 0) := (others => '0'); + begin + tmp := len(15 downto 2); + if(len(1 downto 0) /= "00") then + tmp := std_logic_vector(unsigned(tmp) + to_unsigned(1, tmp'length)); + end if; + return tmp; + end function; + +begin + + rd <= rd_sig; + + -- This process is responsible for MUXing the correct signal to the correct output FIFO + output_prc : process(all) + begin + data_out(output_id) <= data_out_sig; + wr(output_id) <= wr_sig; + end process; + + -- Main State Machine + -- STATE DESCRIPSION + -- IPv4_INIT Initial and idle state. Responsible for checking the buffer timers, and reading the packet length from the input FIFO + -- IPv4_HEADER_1 Parsing first word of IPv4 HEADER + -- IPv4_HEADER_2 Parsing second word of IPv4 HEADER + -- IPv4_HEADER_3 Parsing third word of IPv4 HEADER + -- IPv4_HEADER_4 Parsing fourth word of IPv4 HEADER and writing SRC Addr to output FIFO (if not a fragment) + -- IPv4_HEADER_5 Parsing fifth word of IPv4 HEADER and writing DEST Addr to output FIFO (if not a fragment) + -- IPv4_PAYLOAD_LENGTH Writing packet(payload) length to output FIFO + -- IPv4_PAYLOAD Writing of packet(payload) from input FIFO to output FIFO + -- SKIP_HEADER Skip to beginning of Payload in input FIFO + -- SKIP_PACKET Skip to end of packet in input FIFO + parser_prc : process(all) + variable tmp : integer range 0 to max(MAX_PARALLEL_FRAG,LAYER3_PROTOCOL_NUM) := 0; + begin + --DEFAULT Registered + stage_next <= stage; + packet_length_next <= packet_length; + header_length_next <= header_length; + output_id_next <= output_id; + -- DEFAULT Unregistered + rd_sig <= '0'; + wr_sig <= '0'; + data_out_sig <= (others => '0'); + reset_read_cnt <= '0'; + + case(stage) is + -- Initial/Idle State + when IPv4_INIT => + -- Reset packet Byte Counter + reset_read_cnt <= '1'; + -- Read Packet Length from input + if (empty = '0') then + packet_length_next <= unsigned(data_in(13 downto 0)); + rd_sig <= '1'; + -- Begin Processing + stage_next <= IPv4_HEADER_1; + end if; + -- First IPv4 Header word (Fields: Version, IHL, DSCP, ECN, Total Length) + when IPv4_HEADER_1 => + if (empty = '0') then + rd_sig <= '1'; + -- Wrong IP Version or Packet Length missmatch, skip packet + if (ip_version /= "0100" or normalize_length(ip_length) /= std_logic_vector(packet_length)) then + stage_next <= SKIP_PACKET; + -- Store data and continue Parsing + else + header_length_next <= unsigned(ip_ihl); + stage_next <= IPv4_HEADER_2; + end if; + end if; + -- Second IPv4 Header word (Fields: Identification, Flags, Fragment Offset) + when IPv4_HEADER_2 => + if (empty = '0') then + rd_sig <= '1'; + -- No fragmentation, continue parsing normaly + if (ip_MF_flag = '0' and ip_frag_offset = (ip_frag_offset'reverse_range => '0')) then + stage_next <= IPv4_HEADER_3; + -- IP Fragmentation + else + -- Fragmentation Reassembly disabled, skip packet + stage_next <= SKIP_PACKET; + end if; + end if; + -- Third IPv4 Header word (Fields: TTL, Protocol, Checksum) + when IPv4_HEADER_3 => + if (empty = '0') then + rd_sig <= '1'; + -- Select Output according to Layer 3 Protocol + tmp := LAYER3_PROTOCOL_NUM; + for i in 0 to LAYER3_PROTOCOL_NUM-1 loop + if(ip_protocol = LAYER3_PROTOCOLS(i)) then + tmp := i; + end if; + end loop; + -- No Protocol match, skip packet + if (tmp = LAYER3_PROTOCOL_NUM) then + stage_next <= SKIP_PACKET; + -- Protocol Match, continue parsing + else + output_id_next <= tmp; + stage_next <= IPv4_HEADER_4; + end if; + end if; + -- Fourth IPv4 Header word (Fields: Src Address) + when IPv4_HEADER_4 => + if (empty = '0') then + -- Write Src Address to Output FIFO + if (full(output_id) = '0') then + rd_sig <= '1'; + wr_sig <= '1'; + data_out_sig <= data_in; + -- Continue parsing + stage_next <= IPv4_HEADER_5; + end if; + end if; + -- Fourth IPv4 Header word (Fields: Dest Address) + when IPv4_HEADER_5 => + if (empty = '0') then + -- Write Dst Address to Output FIFO + if (full(output_id) = '0') then + rd_sig <= '1'; + wr_sig <= '1'; + data_out_sig <= data_in; + -- Check Header Size + if (read_cnt /= ("0000000000" & header_length)) then + -- If Header has "Options", skip to Payload + stage_next <= SKIP_HEADER; + else + -- Payload Processing + stage_next <= IPv4_PAYLOAD_LENGTH; + end if; + end if; + end if; + -- Push Payload Length + when IPv4_PAYLOAD_LENGTH => + -- Write Payload Length to Output FIFO + if (full(output_id) = '0') then + wr_sig <= '1'; + data_out_sig(13 downto 0) <= std_logic_vector(packet_length - ("0000000000" & header_length)); + stage_next <= IPv4_PAYLOAD; + end if; + -- Push Payload + when IPv4_PAYLOAD => + -- Write to Output FIFO + if (empty = '0' and full(output_id) = '0') then + rd_sig <= '1'; + wr_sig <= '1'; + data_out_sig <= data_in; + -- End Of Packet + if (read_cnt = packet_length) then + -- Done, process next packet + stage_next <= IPv4_INIT; + end if; + end if; + -- Skip until beginning of payload + when SKIP_HEADER => + if (empty = '0') then + rd_sig <= '1'; + -- End of Header + if(read_cnt = header_length) then + -- Payload Processing + stage_next <= IPv4_PAYLOAD; + end if; + end if; + -- Skip entire packet + when SKIP_PACKET => + if (empty = '0') then + rd_sig <= '1'; + -- End of Packet + if(read_cnt = packet_length) then + -- Continue parsing next packet + stage_next <= IPv4_INIT; + end if; + end if; + when others => + null; + end case; + end process; + + -- Process responsible for counting read words + -- This process uses the actual RAM and FIFO read signals to determine reads + word_counter_prc : process(clk, reset) + begin + if rising_edge(clk) then + -- Reset Read counter + if (reset = '1' or reset_read_cnt = '1') then + read_cnt <= to_unsigned(1, read_cnt'length); + -- Increment read counter each time rd is high + elsif (rd_sig = '1') then + read_cnt <= read_cnt + to_unsigned(1, read_cnt'length); + end if; + end if; + end process; + + -- State Machine Sync Process (Registers) + sync : process(clk) + begin + if rising_edge(clk) then + if (reset = '1') then + stage <= IPv4_INIT; + packet_length <= (others => '0'); + header_length <= (others => '0'); + output_id <= 0; + else + stage <= stage_next; + packet_length <= packet_length_next; + header_length <= header_length_next; + output_id <= output_id_next; + end if; + end if; + end process; + end architecture; \ No newline at end of file diff --git a/src/rtps_handler.vhd b/src/rtps_handler.vhd new file mode 100644 index 0000000..0826c46 --- /dev/null +++ b/src/rtps_handler.vhd @@ -0,0 +1,1030 @@ +library ieee; +use ieee.std_logic_1164.all; +use ieee.numeric_std.all; + +use work.math_pkg.all; +use work.rtps_package.all; + +-- Checksum has to be checked before + +entity rtps_handler is + port ( + clk : in std_logic; -- Input Clock + reset : in std_logic; -- Synchronous Reset + empty : in std_logic; -- Input FIFO empty flag + rd : out std_logic; -- Input FIFO read signal + data_in : in std_logic_vector(31 downto 0); -- Input FIFO data signal + + builtin_output : out BUILTIN_ENDPOINT_TYPE; + builtin_full : in std_logic_vector(NUM_DOMAIN-1 downto 0); + builtin_wr : out std_logic_vector(NUM_DOMAIN-1 downto 0); + user_output : out USER_ENDPOINT_OUTPUT; + user_full : in std_logic_vector(NUM_READERS+NUM_WRITERS-1 downto 0); + user_wr : out std_logic_vector(NUM_READERS+NUM_WRITERS-1 downto 0) + ); +end entity; + +architecture arch of rtps_handler is + + --*****COMPOENENT DECLARATION****** + + --*****CONSTANT DECLARATION***** + -- Minimum Packet Length to consider valid + -- 2 UDP Header 32-bit Words, 5 RTPS Header 32-bit Words + constant MIN_PACKET_LENGTH : integer := 7; + constant MAX_ENDPOINTS : integer := NUM_READERS+NUM_WRITERS; + constant MIN_ACKNACK_SIZE : integer := 24; + constant MIN_INFO_DST_SIZE : integer := 12; + constant MIN_INFO_SRC_SIZE : integer := 20; + constant MIN_INFO_REPLY_SIZE : integer := 1; + constant MIN_INFO_REPLY_IP4_SIZE: integer := 2; + constant MIN_GAP_SIZE : integer := 28; + constant MIN_HEARTBEAT_SIZE : integer := 28; + constant MIN_DATA_SIZE : integer := 20; + + + --GUIDPREFIX(1 downto 0) <= VENDORID; + + --*****TYPE DECLARATION***** + 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, + EXTRACT_LOCATOR_LIST, EXTRACT_LOCATOR, INFO_SRC_HEADER, EXTRACT_DOMAIN_ID, SRC_ENTPOINT, DATA_HEADER, + MATCH_DST_ENDPOINT, PUSH_PAYLOAD_HEADER, DATA_SKIP_HEADER, PUSH_PAYLOAD, CHECK_SUB_END, SKIP_PACKET, + SKIP_SUB); + + + --*****SIGNAL DECLARATION***** + -- FSM state + signal stage, 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) + signal rd_sig : std_logic := '0'; + -- Signal used to reset the word counter + signal reset_read_cnt : std_logic; + -- 32-bit word counter (Counts words read from input fifo) + signal read_cnt : unsigned(13 downto 0) := (others => '0'); + -- 32-bit aligned total packet length + signal packet_length, packet_length_next : unsigned(13 downto 0) := (others => '0'); + signal sub_end, sub_end_next : unsigned(13 downto 0) := (others => '0'); + signal data_header_end, data_header_end_next : unsigned(13 downto 0) := (others => '0'); + signal align_sig, align_sig_next : std_logic_vector(23 downto 0) := (others => '0'); + signal aligned_data_in : std_logic_vector(31 downto 0); + signal aligned_data_in_padded : std_logic_vector(31 downto 0) := (others => '0'); + signal align_offset, align_offset_next : std_logic_vector(1 downto 0) := (others => '0'); + signal offset_latch, 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'); + signal src_addr, src_addr_next : std_logic_vector(31 downto 0) := (others => '0'); + signal src_port, src_port_next : std_logic_vector(15 downto 0) := (others => '0'); + signal is_multicast, is_multicast_next : std_logic := '0'; + signal is_metatraffic, is_metatraffic_next : std_logic := '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; + signal flags, flags_next : std_logic_vector(7 downto 0) := (others => '0'); + signal src_entityid, src_entityid_next : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0'); + signal user_endpoint, user_endpoint_next : std_logic_vector(MAX_ENDPOINTS-1 downto 0) := (others => '0'); + signal builtin_endpoint, builtin_endpoint_next : std_logic := '0'; + signal sub_length, sub_length_next : std_logic_vector(13 downto 0) := (others => '0'); + -- Since Submessages are limited to 2^16 Bytes, we can limit this counter also to 16 bits + signal numlocators, numlocators_next : unsigned(15 downto 0) := (others => '0'); + signal locator_match, locator_match_next : std_logic := '0'; + signal src_is_reader, src_is_reader_next : std_logic := '0'; + signal output_sig : std_logic_vector(31 downto 0) := (others => '0'); + signal wr_sig : std_logic := '0'; + signal opcode, opcode_next : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := (others => '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 + signal cnt, cnt_next : integer range 0 to max(GUIDPREFIX_WIDTH/32, 6) := 0; + + --*****ALIAS DEFINATION***** + -- UDP HEADER + alias udp_src_port : std_logic_vector(15 downto 0) is data_in(31 downto 16); + alias udp_dest_port : std_logic_vector(15 downto 0) is data_in(15 downto 0); + alias udp_length : std_logic_vector(15 downto 0) is data_in(31 downto 16); + alias udp_checksum : std_logic_vector(15 downto 0) is data_in(15 downto 0); + -- RTPS HEADER + alias rtps_version : std_logic_vector(15 downto 0) is data_in(31 downto 16); + alias rtps_vendorid : std_logic_vector(15 downto 0) is data_in(15 downto 0); + -- RTPS SUBMESSAGE HEADER + alias rtps_sub_id : std_logic_vector(7 downto 0) is aligned_data_in(31 downto 24); + alias rtps_sub_flags : std_logic_vector(7 downto 0) is aligned_data_in(23 downto 16); + alias rtps_sub_length : std_logic_vector(15 downto 0) is aligned_data_in(15 downto 0); + -- RTPS SUBMESSAGE FLAGS + alias rtps_sub_endianness : std_logic is flags(0); + alias rtps_sub_multicast : std_logic is flags(1); + alias rtps_sub_qos : std_logic is flags(1); + alias rtps_sub_data : std_logic is flags(2); + alias rtps_sub_key : std_logic is flags(3); + -- RTPS DATA SUBMESSAGE HEADER + alias rtps_sub_data_length : std_logic_vector(15 downto 0) is aligned_data_in(15 downto 0); + + --*****FUNCTION DECLARATION***** + + -- Truncates the lower 2 bits of the input, and if they are not equal zero, adds one to the result. + -- This is used to round the byte length to 32-bit word length. + function normalize_length (len : std_logic_vector(15 downto 0)) return std_logic_vector is + variable tmp : std_logic_vector(13 downto 0) := (others => '0'); + begin + tmp := len(15 downto 2); + if(len(1 downto 0) /= "00") then + tmp := std_logic_vector(unsigned(tmp) + to_unsigned(1, tmp'length)); + end if; + return tmp; + end function; + + -- Depending on the offset argument "off" returns a 32-bit slice from the 56-bit input signal + -- This is used to extract 32-bit aligned words from the input signal + function align_word ( off : std_logic_vector(1 downto 0); + input : std_logic_vector(56 downto 0)) return std_logic_vector is + variable ret : std_logic_vector(31 downto 0) := (others => '0'); + begin + case(off) is + when "00" => + ret := input(31 downto 0); + when "01" => + ret := input(39 downto 8); + when "10" => + ret := input(47 downto 16); + when others => -- "11" + ret := input(55 downto 24); + end case; + return ret; + end function; + + -- Compares argument 'ref' with every elemant of 'ar', and returns the index of the last match. + -- If no match is found, array length is returned. + function match_id_port ( ref : std_logic_vector(UDP_PORT_WIDTH-1 downto 0); + ar : IPv4_PORT_TYPE) return integer is + variable id : integer := 0; + begin + id := ar'length(1); + for i in 0 to ar'length-1 loop + if(ref = ar(i)) then + id := i; + end if; + end loop; + return id; + end function; + function match_id_domain ( ref : std_logic_vector(ENTITYID_WIDTH-1 downto 0); + ar : ENTITYID_TYPE) return integer is + variable id : integer := 0; + begin + id := ar'length(1); + for i in 0 to ar'length-1 loop + if(ref = ar(i)) then + id := i; + end if; + end loop; + return id; + end function; + + -- Returns the 'data' argument either as is, or with reversed Byte order, depending on the + -- 'endianness' argument. + function endian_swap( endianness : std_logic; + data :std_logic_vector) return std_logic_vector is + variable ret : std_logic_vector(data'reverse_range); + begin + -- Assert that Data Signal is Byte aligned + assert (data'length mod 8 = 0) severity failure; + -- Little Endian + if (endianness = '1') then + -- Reverse byte Order + for i in 0 to (data'length/8)-1 loop + ret(i*8+8-1 downto i*8) := data((3-i)*8+8-1 downto (3-i)*8); + end loop; + -- Big Endian + else + ret := data; + end if; + return ret; + end function; + + -- Pads (Overwrites) the 'data' Signal with Zeroes depending on the 'align_offset' argument. + function pad_signal( align_offset: std_logic_vector(1 downto 0); + data : std_logic_vector(31 downto 0)) return std_logic_vector is + variable ret : std_logic_vector(31 downto 0); + begin + ret := data; + case(align_offset) is + when "01" => + ret(23 downto 0) := (others => '0'); + when "10" => + ret(15 downto 0) := (others => '0'); + when "11" => + ret(8 downto 0) := (others => '0'); + when others => --"00" + null; + end case; + return ret; + end function; +begin + + output_prc : process(all) + begin + rd <= rd_sig; + for i in 0 to NUM_DOMAIN-1 loop + builtin_output(i) <= output_sig; + end loop; + for i in 0 to MAX_ENDPOINTS-1 loop + user_output(i) <= output_sig; + end loop; + + builtin_wr <= (others => '0'); + user_wr <= (others => '0'); + if (wr_sig = '1') then + if (builtin_endpoint = '1') then + builtin_wr(domain_id) <= '1'; + else + user_wr <= user_endpoint; + end if; + end if; + end process; + + align_prc : process(all) + variable input : std_logic_vector(55 downto 0) := (others => '0'); + begin + input := align_sig & data_in; + case(align_offset) is + when "01" => + aligned_data_in <= input(55 downto 24); + when "10" => + aligned_data_in <= input(47 downto 16); + when "11" => + aligned_data_in <= input(39 downto 8); + when others => -- "00" + aligned_data_in <= input(31 downto 0); + end case; + end process; + + parse_prc: process(all) + variable tmp : integer range 0 to MAX_ENDPOINTS := 0; + variable tmp_length : std_logic_vector(15 downto 0) := (others => '0'); + begin + --DEFAULT + stage_next <= stage; + reset_read_cnt <= '0'; + cnt_next <= cnt; + align_offset_next <= align_offset; + align_sig_next <= align_sig; + packet_length_next <= packet_length; + sub_end_next <= sub_end; + offset_latch_next <= offset_latch; + src_addr_next <= src_addr; + is_multicast_next <= is_multicast; + src_port_next <= src_port; + is_metatraffic_next <= is_metatraffic; + domain_id_next <= domain_id; + flags_next <= flags; + src_entityid_next <= src_entityid; + user_endpoint_next <= user_endpoint; + builtin_endpoint_next <= builtin_endpoint_next; + return_stage_next <= return_stage; + numlocators_next <= numlocators; + locator_match_next <= locator_match; + sub_length_next <= sub_length; + src_is_reader_next <= src_is_reader; + output_sig <= (others => '0'); + wr_sig <= '0'; + opcode_next <= opcode; + data_header_end_next <= data_header_end; + data_header_offset_latch_next <= data_header_offset_latch; + is_data_next <= is_data; + data_header_length_next <= data_header_length; + + case(stage) is + -- Initial/Idle State + -- Src Addr + when SRC_ADDR_HEADER => + if (empty = '0') then + rd_sig <= '1'; + -- Latch Src Address + src_addr_next <= data_in; + -- TODO: Reset Flags + is_multicast_next <= '0'; + is_metatraffic_next <= '0'; + -- Next Stage + stage_next <= DEST_ADDR_HEADER; + end if; + -- Dest Addr + when DEST_ADDR_HEADER => + if (empty = '0') then + rd_sig <= '1'; + -- Check Destination Address + if (data_in = IPv4_UNICAST_ADDRESS or data_in = DEFAULT_IPv4_MULTICAST_ADDRESS) then + -- Latch if Addr is Multicast + if (data_in = DEFAULT_IPv4_MULTICAST_ADDRESS) then + is_multicast_next <= '1'; + end if; + -- Next Stage + stage_next <= UDP_HEADER_1; + -- packet not for us, skip + else + stage_next <= SKIP_PACKET; + end if; + end if; + when LEN_HEADER => + -- Reset packet Byte Counter + reset_read_cnt <= '1'; + if (empty = '0') then + rd_sig <= '1'; + -- Read Packet Length from input + packet_length_next <= unsigned(data_in(13 downto 0)); + -- Check Packet Length + if(to_integer(unsigned(data_in(13 downto 0))) < MIN_PACKET_LENGTH) then + stage_next <= SKIP_PACKET; + else + -- Begin Processing + stage_next <= UDP_HEADER_1; + end if; + end if; + -- First UDP Header word (Fields: Src Port, Dest Port) + when UDP_HEADER_1 => + if (empty = '0') then + rd_sig <= '1'; + -- Latch Src Port + src_port_next <= udp_src_port; + -- Default Next Stage + stage_next <= SKIP_PACKET; + -- Check if Dest Port is valid, if not Skip Packet + if (is_multicast = '1') then + -- Check if Metatraffic (via Mutlicast) + tmp := match_id_port(udp_dest_port, META_IPv4_MULTICAST_PORT); + if (tmp /= MAX_ENDPOINTS) then + is_metatraffic_next <= '1'; + domain_id_next <= tmp; + stage_next <= UDP_HEADER_2; + -- Check if User Traffic (via Multicast) + else + tmp := match_id_port(udp_dest_port, USER_IPv4_MULTICAST_PORT); + if (tmp /= MAX_ENDPOINTS) then + stage_next <= UDP_HEADER_2; + domain_id_next <= tmp; + end if; + end if; + else + -- Check if Metatraffic (via Unicast) + tmp := match_id_port(udp_dest_port, META_IPv4_UNICAST_PORT); + if (tmp /= MAX_ENDPOINTS) then + is_metatraffic_next <= '1'; + domain_id_next <= tmp; + stage_next <= UDP_HEADER_2; + -- Check if User Traffic (via Unicast) + else + tmp := match_id_port(udp_dest_port, USER_IPv4_UNICAST_PORT); + if (tmp /= MAX_ENDPOINTS) then + stage_next <= UDP_HEADER_2; + domain_id_next <= tmp; + end if; + end if; + end if; + end if; + -- Second UDP Header Word (Fields: Length, Checksum) + when UDP_HEADER_2 => + if (empty = '0') then + rd_sig <= '1'; + -- If UPD header length does not match actual packet length, skip packet + if (normalize_length(udp_length) /= std_logic_vector(packet_length)) then + stage_next <= SKIP_PACKET; + else + -- Next Stage + stage_next <= RTPS_HEADER_1; + end if; + end if; + -- First RTPS Header word (Fields: Protocolld) + when RTPS_HEADER_1 => + if (empty = '0') then + rd_sig <= '1'; + -- If underlying Protocol is not RTPS, skip packet + if(data_in /= PROTOCOL_RTPS) then + stage_next <= SKIP_PACKET; + -- Continue Parsing + else + -- Next Stage + stage_next <= RTPS_HEADER_2; + end if; + end if; + -- Second RTPS Header word (Fields: Protocol Version, Vendor ID) + when RTPS_HEADER_2 => + if (empty = '0') then + rd_sig <= '1'; + -- If RTPS Protocol Major Version is not 2, skip packet + if(rtps_version(15 downto 8) /= PROTOCOLVERSION_2_4(15 downto 8)) then + stage_next <= SKIP_PACKET; + -- Continue Parsing + else + -- Reset GP Counter + cnt_next <= 1; + -- Next Stage + stage_next <= RTPS_HEADER_3; + + end if; + end if; + -- Rest of RTPS Header (Fields: GUID Prefix) + when RTPS_HEADER_3 => + if (empty = '0') then + rd_sig <= '1'; + -- Sender GUID_Prefix + -- Since we already have the Src Addr and Port, we do not need the + -- Guid_prefix of the source. + --TODO <= data_in; + if (cnt = GUIDPREFIX_WIDTH/32) then + -- Next Stage + stage_next <= RTPS_SUB_HEADER; + else + cnt_next <= cnt + 1; + end if; + end if; + -- NOTE: From here on, due to the nature of the RTPS Protocol, 32-bit word alignement + -- is not guaranteed, and has to be handled. + -- NOTE: This Entity only decodes the Destination Endpoint of the Submessages. The decoding + -- of the actual content is done in the respective Endpoints. + -- This decision was made because the Endpoints need to know the length of the contents beforehand, + -- and determining the length of the variable size Submessage contents adds unnecessary complexity + -- and latency. + -- RTPS Submessage Header (Fields: Submessage ID, Flags, Submessage Length) + when RTPS_SUB_HEADER => + if (empty = '0') then + rd_sig <= '1'; + + -- TODO: Still valid? + -- The case that a submessage is the last one (Length=0) is silently handled + -- by the 'SKIP_SUB' stage, which is taken after every Submessage (in order to skip + -- unknown parts of the Submessage and keep backwards Compatibility [see DDSI-RTPS 2.3 Section 8.3.4.1]). + + --DEFAULT + src_is_reader_next <= '0'; + is_data_next <= '0'; + + case (rtps_sub_id) is + -- INFO_DST (Writer -> Reader, Update Destination GUID Prefix) + when SID_INFO_DST => + -- Check Length + if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_INFO_DST_SIZE)) then + -- Invalid Submessage, Skip Packet (see DDSI-RTPS 2.3 Section 8.3.4.1) + stage_next <= SKIP_PACKET; + else + -- Initialise Counter + cnt_next <= 1; + -- Next Stage + stage_next <= EXTRACT_DOMAIN_ID; + return_stage_next <= CHECK_SUB_END; + end if; + -- INFO_SRC (RTPS Header in Submessage form) + when SID_INFO_SRC => + -- Check Length + if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_INFO_SRC_SIZE)) then + -- Invalid Submessage, Skip Packet (see DDSI-RTPS 2.3 Section 8.3.4.1) + stage_next <= SKIP_PACKET; + else + -- Initialise Counter + cnt_next <= 1; + -- Next Stage + stage_next <= INFO_SRC_HEADER; + end if; + -- INFO_TS (Source Timestamp) + when SID_INFO_TS => + -- IGNORE + stage_next <= CHECK_SUB_END; + -- INFO_REPLY (Source Port and Address) + when SID_INFO_REPLY => + -- Check Length + if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_INFO_REPLY_SIZE)) then + -- Invalid Submessage, Skip Packet (see DDSI-RTPS 2.3 Section 8.3.4.1) + stage_next <= SKIP_PACKET; + else + -- Parse Locators + stage_next <= EXTRACT_LOCATOR_LIST; + end if; + -- INFO_REPLY (Source Port and Address) + when SID_INFO_REPLY_IP4 => + -- 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 + -- Invalid Submessage, Skip Packet (see DDSI-RTPS 2.3 Section 8.3.4.1) + stage_next <= SKIP_PACKET; + else + -- Parse Locators + stage_next <= SKIP_SUB; + end if; + -- Heartbeat (Writer -> Reader, Available SeqNum) + when SID_HEARTBEAT => + -- Check Length + if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_HEARTBEAT_SIZE)) then + -- Invalid Submessage, Skip Packet (see DDSI-RTPS 2.3 Section 8.3.4.1) + stage_next <= SKIP_PACKET; + else + -- Next Stage + stage_next <= MATCH_DST_ENDPOINT; + return_stage_next <= SRC_ENTPOINT; + end if; + -- AckNack (Reader -> Writer, Request SeqNum) + when SID_ACKNACK => + -- Check Length + if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_ACKNACK_SIZE)) then + -- Invalid Submessage, Skip Packet (see DDSI-RTPS 2.3 Section 8.3.4.1) + stage_next <= SKIP_PACKET; + else + -- Mark if destination is for writers + src_is_reader_next <= '1'; + -- Next Stage + stage_next <= SRC_ENTPOINT; + end if; + -- GAP (Writer -> Reader, Invalidate SeqNum) + when SID_GAP => + -- Check Length + if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_GAP_SIZE)) then + -- Invalid Submessage, Skip Packet (see DDSI-RTPS 2.3 Section 8.3.4.1) + stage_next <= SKIP_PACKET; + else + -- Next Stage + stage_next <= MATCH_DST_ENDPOINT; + return_stage_next <= SRC_ENTPOINT; + end if; + -- DATA (Writer -> Reader, SeqNum+Data) + when SID_DATA => + -- Check Length + if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_DATA_SIZE)) then + -- Invalid Submessage, Skip Packet (see DDSI-RTPS 2.3 Section 8.3.4.1) + stage_next <= SKIP_PACKET; + else + -- Next Stage (DEFAULT) + stage_next <= DATA_HEADER; + is_data_next <= '1'; + -- Sanity Check + if (rtps_sub_qos = '0' and rtps_sub_data = '0' and rtps_sub_key = '0') then + -- Submessage has no Data, skip + stage_next <= SKIP_SUB; + elsif (rtps_sub_data = '1' and rtps_sub_key = '1') then + -- Invalid Submessage, skip (see DDSI-RTPS 2.3 Section 9.4.5.3.1) + stage_next <= SKIP_SUB; + end if; + end if; + when SID_HEARTBEAT_FRAG => + -- IGNORE + stage_next <= SKIP_SUB; + when SID_NACK_FRAG => + -- IGNORE + stage_next <= SKIP_SUB; + when SID_DATA_FRAG => + -- IGNORE + stage_next <= SKIP_SUB; + -- PAD (Variable Size Padding) + when SID_PAD => + stage_next <= CHECK_SUB_END; + -- Unknown ID, skip submessage + when others => + -- NOTE: DDSI-RTPS 2.3 only defines the 'INFO_TS' and 'PAD' Submessages to be able to have a zero Submessage Length. + -- If an unknown Submessage Type is encountered that also can have a valid zero Submessage length, this will be + -- seen here as "Last Submessage", and the complete rest of the Packet will be Skipped. + stage_next <= SKIP_SUB; + end case; + -- Latch flags + flags_next <= rtps_sub_flags; + -- Latch Submessage ID + opcode_next <= rtps_sub_id; + -- Check if Last Submessage + if (rtps_sub_length = (rtps_sub_length'reverse_range => '0')) then + -- EXCEPTION: INFO_TS and PAD can have valid zero Submessage Length + if (rtps_sub_id /= SID_PAD and rtps_sub_id /= SID_INFO_TS) then + -- Fix Submessage End Position + sub_end_next <= packet_length; + -- Calculate Submessage Length + -- 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)); + end if; + else + -- Latch Submessage End + -- TODO: The below conditional +1 adder should be solved with a carry-in + -- TODO: Use GP Adder? + sub_end_next <= read_cnt + unsigned(normalize_length(endian_swap(rtps_sub_endianness,rtps_sub_length))); + -- Latch Submessage Size (+1 to adjust to the format recognised by the Endpoints) + if (rtps_sub_id = SID_DATA) then + -- (+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)); + else + -- (+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)); + end if; + end if; + -- Latch Byte offset of next Header + offset_latch_next <= std_logic_vector(unsigned(align_offset) + unsigned(rtps_sub_length(1 downto 0))); + -- Latch Input for alignment purposes + align_sig_next <= data_in(23 downto 0); + end if; + when EXTRACT_LOCATOR_UDPv4_1 => + if (empty = '0') then + rd_sig <= '1'; + -- Latch Source Port + src_port_next <= endian_swap(rtps_sub_endianness, aligned_data_in)(src_port_next'length-1 downto 0); + -- Next Stage + stage_next <= EXTRACT_LOCATOR_UDPv4_2; + -- Latch Input for alignment purposes + align_sig_next <= data_in(23 downto 0); + end if; + when EXTRACT_LOCATOR_UDPv4_2 => + if (empty = '0') then + rd_sig <= '1'; + -- Latch Src Addr + -- TODO: Could it be that the address is INVALID, in favor of the possibly following Multicast Address? + src_addr_next <= endian_swap(rtps_sub_endianness, aligned_data_in); + -- NOTE: The Multicast locator that could possibly follow is ignored + -- Next Stage + stage_next <= CHECK_SUB_END; + -- Latch Input for alignment purposes + align_sig_next <= data_in(23 downto 0); + end if; + when EXTRACT_LOCATOR_LIST => + if (empty = '0') then + rd_sig <= '1'; + -- Latch Number of Locators + numlocators_next <= unsigned(endian_swap(rtps_sub_endianness, aligned_data_in)(numlocators_next'length-1 downto 0)); + -- Initialise counter + cnt_next <= 1; + -- Next Stage + stage_next <= EXTRACT_LOCATOR; + -- Latch Input for alignment purposes + align_sig_next <= data_in(23 downto 0); + end if; + when EXTRACT_LOCATOR => + -- Check Number of Locators + if (numlocators = (numlocators'reverse_range => '0')) then + -- Check if Multicast Locator List exists + if (rtps_sub_multicast = '1') then + -- Parse Multicast Locator List + stage_next <= EXTRACT_LOCATOR_LIST; + -- Reset Flags to prevent loop + flags_next <= (others => '0'); + -- No further Locators, next Submessage + else + stage_next <= CHECK_SUB_END; + end if; + else + if (empty = '0') then + rd_sig <= '1'; + -- Increment Counter (Default) + cnt_next <= cnt + 1; + -- Locator Kind + if (cnt = 1) then + -- Check if UDPv4 Locator + if (endian_swap(rtps_sub_endianness, aligned_data_in) = LOCATOR_KIND_UDPv4) then + locator_match_next <= '1'; + else + locator_match_next <= '0'; + end if; + -- Locator Port + elsif (cnt = 2) then + if (locator_match = '1') then + -- Latch Source Port + src_port_next <= endian_swap(rtps_sub_endianness, aligned_data_in)(src_port_next'length-1 downto 0); + end if; + -- Locator Addr (IPv4) + elsif (cnt = 6) then + if (locator_match = '1') then + -- Latch Src Addr + src_addr_next <= endian_swap(rtps_sub_endianness, aligned_data_in); + -- Extract only first matching Locator and ignore the rest + stage_next <= CHECK_SUB_END; + end if; + -- Last Word of Locator + -- Decrement Locator Count + numlocators_next <= numlocators - 1; + -- Reset Counter + cnt_next <= 1; + end if; + -- Latch Input for alignment purposes + align_sig_next <= data_in(23 downto 0); + end if; + end if; + when INFO_SRC_HEADER => + if (empty = '0') then + rd_sig <= '1'; + -- Second Word of INFO_SRC Submessage + if (cnt = 2) then + -- Check Major Protocol Version + if (aligned_data_in(31 downto 24) /= PROTOCOLVERSION_2_4(15 downto 8)) then + -- Protocol Supported, skip rest of Submessage + stage_next <= SKIP_SUB; + else + -- Protocol not supported, skip Packet + stage_next <= SKIP_PACKET; + end if; + else + -- TODO: Check how many addres are synthesized for cnt/cnt_next + cnt_next <= cnt + 1; + end if; + -- Latch Input for alignment purposes + align_sig_next <= data_in(23 downto 0); + end if; + -- Extract Domain/participant ID from GUID Prefix + when EXTRACT_DOMAIN_ID => + if (empty = '0') then + rd_sig <= '1'; + if (cnt = GUIDPREFIX_WIDTH/32) then + -- NOTE: Assumes DOMAIN_ID_WIDTH is <= 32 + assert (DOMAIN_ID_WIDTH <= 32) severity FAILURE; + -- Latch Domain ID + domain_id_next <= to_integer(unsigned(endian_swap(rtps_sub_endianness, aligned_data_in))); + -- Next Stage (Dynamic) + stage_next <= return_stage; + else + -- TODO: Check how many addres are synthesized for cnt/cnt_next + cnt_next <= cnt + 1; + end if; + -- Latch Input for alignment purposes + align_sig_next <= data_in(23 downto 0); + end if; + when SRC_ENTPOINT => + if (empty = '0') then + rd_sig <= '1'; + -- Latch src Entity ID + src_entityid_next <= aligned_data_in; + -- Next Stage + if (src_is_reader = '1') then + stage_next <= MATCH_DST_ENDPOINT; + return_stage_next <= PUSH_PAYLOAD_HEADER; + else + stage_next <= PUSH_PAYLOAD_HEADER; + end if; + -- Initialise Counter + cnt_next <= 1; + -- Latch Input for alignment purposes + align_sig_next <= data_in(23 downto 0); + end if; + when DATA_HEADER => + if (empty = '0') then + rd_sig <= '1'; + -- Latch Data Header Length + data_header_offset_latch_next <= std_logic_vector(unsigned(align_offset) + unsigned(endian_swap(rtps_sub_endianness, rtps_sub_data_length)(1 downto 0))); + -- Latch offset + data_header_end_next <= read_cnt + unsigned(normalize_length(endian_swap(rtps_sub_endianness, rtps_sub_data_length))); + -- Fix Payload Size + sub_length_next <= std_logic_vector(unsigned(sub_length) - unsigned(normalize_length(endian_swap(rtps_sub_endianness, rtps_sub_data_length)))); + -- Next Stage + stage_next <= MATCH_DST_ENDPOINT; + return_stage_next <= SRC_ENTPOINT; + -- Latch Input for alignment purposes + align_sig_next <= data_in(23 downto 0); + end if; + when MATCH_DST_ENDPOINT => + if (empty = '0') then + rd_sig <= '1'; + -- Next Stage (Default, Dynamic) + stage_next <= return_stage; + -- *Check Dest Entity ID* + -- Default Values + user_endpoint_next <= (others => '0'); + builtin_endpoint_next <= '0'; + -- Target ALL Endpoints of Domain + if (aligned_data_in = ENTITYID_UNKNOWN) then + if (is_metatraffic = '1') then + builtin_endpoint_next <= '1'; + else + user_endpoint_next <= DOMAIN_ENDPOINT_MAP(domain_id); + -- Mark Only Writers + if (src_is_reader = '1') then + user_endpoint_next(MAX_ENDPOINTS-1 downto MAX_ENDPOINTS-NUM_READERS) <= (others => '0'); + -- Mark Only Readers + else + -- TODO: Needed? + if (NUM_WRITERS /= 0) then + user_endpoint_next(NUM_WRITERS-1 downto 0) <= (others => '0'); + end if; + end if; + end if; + -- Target Built-In Endpoints + elsif (is_metatraffic = '1' and aligned_data_in(7 downto 6) = BUILT_IN_ENTITY) then + builtin_endpoint_next <= '1'; + -- Match User Entity ID + elsif (is_metatraffic = '0') then + tmp := match_id_domain(aligned_data_in, ENTITYID); + -- Entity non existent, skip Submessage + if (tmp = MAX_ENDPOINTS) then + stage_next <= SKIP_SUB; + -- Entity ID Match + else + -- NOTE: Inserting a Sanity Check here that prevents Reader-Reader and Writer-Writer + -- Communication would prevent unnecessary FIFO clutter. But the check needs + -- to compare if 'tmp' is inside a range, which was deemed more complicated than + -- necessary, since the Endpoints can handle traffic not destined to them. + user_endpoint_next(tmp) <= '1'; + builtin_endpoint_next <= '0'; + end if; + -- Destination Unreachable, skip Submessage + else + stage_next <= SKIP_SUB; + end if; + -- Latch Input for alignment purposes + align_sig_next <= data_in(23 downto 0); + end if; + when PUSH_PAYLOAD_HEADER => + -- NOTE: This is a synchronised push on potentially multiple output FIFOs. + -- If one FIFO gets full, the process stalls for all FIFOs. + -- If Output FIFOs not full + if (builtin_endpoint = '1' and builtin_full(domain_id) = '0') or (builtin_endpoint = '0' and ((user_endpoint and user_full) = (user_endpoint'reverse_range => '0'))) then + -- Increment Counter + cnt_next <= cnt + 1; + case (cnt) is + when 1 => + output_sig(sub_length'length-1 downto 0) <= sub_length; + wr_sig <= '1'; + when 2 => + output_sig <= opcode & flags & src_port; + wr_sig <= '1'; + when 3 => + output_sig <= src_addr; + wr_sig <= '1'; + when 4 => + output_sig <= src_entityid; + wr_sig <= '1'; + -- Next Stage + if (is_data = '1') then + stage_next <= DATA_SKIP_HEADER; + -- Reset Counter + cnt_next <= 1; + else + stage_next <= PUSH_PAYLOAD; + end if; + when others => + null; + end case; + end if; + when DATA_SKIP_HEADER => + if (empty = '0') then + -- Known DATA Header Data (Sequence Number Field) + if (cnt <= 2) then + -- If Output FIFOs not full + if (builtin_endpoint = '1' and builtin_full(domain_id) = '0') or (builtin_endpoint = '0' and ((user_endpoint and user_full) = (user_endpoint'reverse_range => '0'))) then + rd_sig <= '1'; + -- Push Payload + output_sig <= endian_swap(rtps_sub_endianness, aligned_data_in); + wr_sig <= '1'; + -- Latch Input for alignment purposes + align_sig_next <= data_in(23 downto 0); + -- Increment Counter + cnt_next <= cnt + 1; + end if; + -- Uknown Header Portion + else + -- Skip Header Words + rd_sig <= '1'; + -- Latch Input for alignment purposes + align_sig_next <= data_in(23 downto 0); + end if; + -- End of Data Header + if (read_cnt = data_header_end) then + -- Push Submessage Contents + stage_next <= PUSH_PAYLOAD; + -- Fix alignement + align_offset <= data_header_offset_latch; + end if; + end if; + when PUSH_PAYLOAD => + if (empty = '0') then + -- If Output FIFOs not full + if (builtin_endpoint = '1' and builtin_full(domain_id) = '0') or (builtin_endpoint = '0' and ((user_endpoint and user_full) = (user_endpoint'reverse_range => '0'))) then + rd_sig <= '1'; + -- Push Payload + -- Last Payload Word + if (read_cnt = sub_end) then + -- Push Last Word of Payload with padding + output_sig <= pad_signal(offset_latch, aligned_data_in); + wr_sig <= '1'; + -- Begin parsing of next submessage + stage_next <= RTPS_SUB_HEADER; + -- Fix alignement + align_offset <= offset_latch; + else + -- Push Normal (Endianness is not handled here) + output_sig <= aligned_data_in; + wr_sig <= '1'; + end if; + -- Latch Input for alignment purposes + align_sig_next <= data_in(23 downto 0); + end if; + end if; + -- Check Submessage/Packet End + when CHECK_SUB_END => + -- NOTE: Due to the behavior of the read counter, the length is +1 in this stage. + -- So we can either check the length+1 for equality, or do a relative comparison on length. + -- End of Packet (No further Submessages) + if (read_cnt > packet_length) then + -- Continue parsing next packet + stage_next <= SRC_ADDR_HEADER; + -- End of Submessage + elsif (read_cnt > sub_end) then + -- Begin parsing of next submessage + stage_next <= RTPS_SUB_HEADER; + -- Fix alignement + align_offset <= offset_latch; + -- Submessage has still "unknown" Content + else + stage_next <= SKIP_SUB; + end if; + when SKIP_PACKET => + if (empty = '0') then + rd_sig <= '1'; + -- End of Packet + if (read_cnt = packet_length) then + -- Continue parsing next packet + stage_next <= SRC_ADDR_HEADER; + end if; + end if; + when SKIP_SUB => + if (empty = '0') then + rd_sig <= '1'; + -- End of Packet (No further Submessages) + if (read_cnt = packet_length) then + -- Continue parsing next packet + stage_next <= SRC_ADDR_HEADER; + -- End of Submessage + elsif (read_cnt = sub_end) then + -- Begin parsing of next submessage + stage_next <= RTPS_SUB_HEADER; + -- Fix alignement + align_offset <= offset_latch; + end if; + -- Latch Input for alignment purposes + align_sig_next <= data_in(23 downto 0); + end if; + when others => + null; + end case; + end process; + + -- Process responsible for counting read words + -- This process uses the actual FIFO read signals to determine reads + word_counter_prc : process(clk, reset) + begin + if rising_edge(clk) then + -- Reset Read counter + if (reset = '1' or reset_read_cnt = '1') then + read_cnt <= to_unsigned(1, read_cnt'length); + -- Increment read counter each time rd is high + elsif (rd_sig = '1') then + read_cnt <= read_cnt + to_unsigned(1, read_cnt'length); + end if; + end if; + end process; + + sync: process(clk) + begin + if rising_edge(clk) then + if (reset = '1') then + stage <= SRC_ADDR_HEADER; + return_stage <= SRC_ADDR_HEADER; + cnt <= 0; + align_offset <= (others => '0'); + align_sig <= (others => '0'); + packet_length <= (others => '0'); + sub_end <= (others => '0'); + offset_latch <= (others => '0'); + src_addr <= (others => '0'); + src_port <= (others => '0'); + domain_id <= 0; + flags <= (others => '0'); + src_entityid <= (others => '0'); + user_endpoint <= (others => '0'); + numlocators <= (others => '0'); + sub_length <= (others => '0'); + opcode <= (others => '0'); + data_header_end <= (others => '0'); + data_header_offset_latch <= (others => '0'); + data_header_length <= (others => '0'); + is_data <= '0'; + src_is_reader <= '0'; + locator_match <= '0'; + builtin_endpoint <= '0'; + is_metatraffic <= '0'; + is_multicast <= '0'; + else + stage <= stage_next; + return_stage <= return_stage_next; + cnt <= cnt_next; + align_offset <= align_offset_next; + align_sig <= align_sig_next; + packet_length <= packet_length_next; + sub_end <= sub_end_next; + offset_latch <= offset_latch_next; + src_addr <= src_addr_next; + src_port <= src_port_next; + domain_id <= domain_id_next; + flags <= flags_next; + src_entityid <= src_entityid_next; + user_endpoint <= user_endpoint_next; + numlocators <= numlocators_next; + sub_length <= sub_length_next; + opcode <= opcode_next; + data_header_end <= data_header_end_next; + data_header_offset_latch <= data_header_offset_latch_next; + data_header_length <= data_header_length_next; + is_data <= is_data_next; + src_is_reader <= src_is_reader_next; + locator_match <= locator_match_next; + builtin_endpoint <= builtin_endpoint_next; + is_metatraffic <= is_metatraffic_next; + is_multicast <= is_multicast_next; + end if; + end if; + end process; + +end architecture; \ No newline at end of file diff --git a/src/rtps_handler.vhd.BAK b/src/rtps_handler.vhd.BAK new file mode 100644 index 0000000..f4742dc --- /dev/null +++ b/src/rtps_handler.vhd.BAK @@ -0,0 +1,516 @@ +library ieee; +use ieee.std_logic_1164.all; +use ieee.numeric_std.all; + +use work.math_pkg.all; + +entity rtps_handler is + generic( + + ); + port ( + clk : in std_logic; -- Input Clock + reset : in std_logic; -- Synchronous Reset + empty : in std_logic; -- Input FIFO empty flag + rd : out std_logic; -- Input FIFO read signal + data_in : in std_logic_vector(31 downto 0); -- Input FIFO data signal + + ); +end entity; + +architecture arch of rtps_handler is + + --*****COMPOENENT DECLARATION****** + entity adder is + generic ( + PIPELINE_STAGES : integer := 1; + DATA_WIDTH : integer := 32 + ); + port ( + clk : in std_logic; + reset : in std_logic; + cin : in std_logic; + A : in std_logic_vector(DATA_WIDTH-1 downto 0); + B : in std_logic_vector(DATA_WIDTH-1 downto 0); + RES : out std_logic_vector(DATA_WIDTH-1 downto 0); + cout : out std_logic + ); + end entity; + + --*****CONSTANT DECLARATION***** + -- Minimum Packet Length to consider valid + -- 2 UDP Header 32-bit Words, 5 RTPS Header 32-bit Words + constant MIN_PACKET_LENGTH : integer := 7; + constant MAX_ENDPOINTS : integer := NUM_READERS+NUM_WRITERS; + + + --GUIDPREFIX(1 downto 0) <= VENDORID; + + --*****TYPE DECLARATION***** + type STAGE_TYPE is (INIT, SRC_ADDR, DEST_ADDR, UDP_HEADER_1, UDP_HEADER_2, RTPS_HEADER_1, + RTPS_HEADER_2, RTPS_HEADER_3, RTPS_HEADER_4, SKIP_PACKET); + + + --*****SIGNAL DECLARATION***** + -- FSM state + signal stage, stage_next : PARSER_STAGE_TYPE := INIT; + -- Intermediate input read signal. (Read from output port not allowed) + signal rd_sig : std_logic := '0'; + -- Signal used to reset the word counter + signal reset_read_cnt : std_logic; + -- 32-bit word counter (Counts words read from input fifo) + signal read_cnt : unsigned(13 downto 0) := (others => '0'); + -- 32-bit aligned total packet length + signal packet_length, packet_length_next : unsigned(13 downto 0) := (others => '0'); + signal sub_length, sub_length_next : unsigned(13 downto 0) := (others => '0'); + signal align_sig, align_sig_next : std_logic_vector(23 downto 0) := (others => '0'); + signal aligned_data_in : std_logic_vector(31 downto 0); + signal align_offset, align_offset_next : std_logic_vector(1 downto 0) := (others => '0'); + signal offset_latch, offset_latch_next : std_logic_vector(1 downto 0) := (others => '0'); + signal src_addr, src_addr_next : std_logic_vector(31 downto 0) := (others => '0'); + signal src_port, src_port_next : std_logic_vector(15 downto 0) := (others => '0'); + signal is_multicast, is_multicast_next : std_logic := '0'; + signal is_metatraffic, is_metatraffic_next : std_logic := '0'; + signal participant_id, participant_id_next : integer range 0 to NUM_DOMAIN-1 := 0; + signal flags, flags_next : std_logic_vector(7 downto 0) := (others => '0'); + -- General purpose counter + signal cnt, cnt_next : integer := 0; --TODO: Range + -- Adder Signals + signal add_res, add_a, add_b : std_logic_vector(31 downto 0) := (others => '0'); + signal add_cin, add_cout : std_logic := '0'; + + --*****ALIAS DEFINATION***** + -- UDP HEADER + alias udp_src_port : std_logic_vector(15 downto 0) is data_in(31 downto 16); + alias udp_dest_port : std_logic_vector(15 downto 0) is data_in(15 downto 0); + alias udp_length : std_logic_vector(15 downto 0) is data_in(31 downto 16); + alias udp_checksum : std_logic_vector(15 downto 0) is data_in(15 downto 0); + -- RTPS HEADER + alias rtps_version : std_logic_vector(15 downto 0) is data_in(31 downto 16); + alias rtps_vendorid : std_logic_vector(15 downto 0) is data_in(15 downto 0); + -- RTPS SUBMESSAGE HEADER + alias rtps_sub_id : std_logic_vector(7 downto 0) is aligned_data_in(31 downto 24); + alias rtps_sub_flags : std_logic_vector(7 downto 0) is aligned_data_in(23 downto 16); + alias rtps_sub_length : std_logic_vector(7 downto 0) is aligned_data_in(15 downto 0); + -- ACKNACK + alias rtps_acknack_final : std_logic is rtps_sub_flags(1); + -- MISC + + --*****FUNCTION DECLARATION***** + + -- Truncates the lower 2 bits of the input, and if they are not equal zero, adds one to the result. + -- This is used to round the byte length to 32-bit word length. + function normalize_length (len : std_logic_vector(15 downto 0)) return std_logic_vector is + variable tmp : std_logic_vector(13 downto 0) := (others => '0'); + begin + tmp := len(15 downto 2); + if(len(1 downto 0) /= "00") then + tmp := std_logic_vector(unsigned(tmp) + to_unsigned(1, tmp'length)); + end if; + return tmp; + end function; + + -- Depending on the offset argument "off" returns a 32-bit slice from the 56-bit input signal + -- This is used to extract 32-bit aligned words from the input signal + function align_word ( off : std_logic_vector(1 downto 0), + input : std_logic_vector(56 downto 0)) return std_logic_vector is + variable ret : std_logic_vector(31 downto 0) := (others => '0'); + begin + case(off) is + when "00" => + ret := input(31 downto 0); + when "01" => + ret := input(39 downto 8); + when "10" => + ret := input(47 downto 16); + when "11" => + ret := input(55 downto 24); + end case; + return ret; + end function; + + -- Compares argument 'ref' with every elemant of 'ar', and returns the index of the last match. + -- If no match is found, array length is returned. + function match_domain_id ( ref : std_logic_vector(UDP_PORT_WIDTH-1 downto 0), + ar : IPv4_PORT_TYPE) return integer is + variable id : integer := 0; + begin + id := ar'length(1); + for i in 0 to ar'length-1 loop + if(ref = ar(i)) then + id := i; + end if; + end loop; + return id; + end function; + + -- Returns the 'data' argument either as is, or with reversed Byte order, depending on the + -- 'endianness' argument. + function endian_swap( endianness : std_logic, + data :std_logic_vector(31 downto 0)) return std_logic_vector is + variable ret : std_logic_vector(31 downto 0); + begin + -- Little Endian + if (endianness = '1') then + -- Reverse byte Order + for i in 0 to 3 loop + ret(i*8+8-1 downto i*8) := data((3-i)*8+8-1 downto (3-i)*8); + end loop; + -- Big Endian + else + ret := data; + end if; + return ret; + end function; +begin + + rd <= rd_sig; + + align_prc : process(all) + variable input : std_logic_vector(55 downto 0) := (others => '0'); + begin + input := align_sig & data_in; + case(align_offset) is + when "00" => + aligned_data_in <= input(31 downto 0); + when "01" => + aligned_data_in <= input(39 downto 8); + when "10" => + aligned_data_in <= input(47 downto 16); + when "11" => + aligned_data_in <= input(55 downto 24); + end case; + end process; + + checksum_adder : adder + generic map ( + PIPELINE_STAGES => 1, + DATA_WIDTH => 32 + ) + port map( + clk => clk, + reset => reset, + cin => add_cin, + A => add_a, + B => add_b, + RES => add_res, + cout => add_cout + ); + end entity; + + parse_prc: process(all) + variable tmp : integer range 0 to MAX_ENDPOINTS := 0; + begin + --DEFAULT + stage_next <= stage; + reset_read_cnt <= '0'; + add_a <= (others => '0'); + add_b <= (others => '0'); + add_cin <= '0'; + cnt_next <= count; + align_offset_next <= align_offset; + align_sig_next <= align_sig; + packet_length_next <= packet_length; + sub_length_next <= sub_length; + offset_latch_next <= offset_latch; + src_addr_next <= src_addr; + is_multicast_next <= is_multicast; + src_port_next <= src_port; + is_metatraffic_next <= is_metatraffic; + participant_id_next <= participant_id; + flags_next <= flags; + + case(stage) is + -- Initial/Idle State + -- Src Addr + when SRC_ADDR => + if (empty = '0') then + rd_sig <= '1'; + -- Latch Src Address + src_addr_next <= data_in; + -- Initiate Checksum Calculation + add_a <= data_in; + -- TODO: Reset Flags + is_multicast_next <= '0'; + is_metatraffic_next <= '0'; + -- Next Stage + stage_next <= DEST_ADDR; + end if; + -- Dest Addr + when DEST_ADDR => + if (empty = '0') then + rd_sig <= '1'; + -- Check Destination Address + if (data_in = IPv4_UNICAST_ADDRESS or data_in = DEFAULT_IPv4_MULTICAST_ADDRESS) then + -- Latch if Addr is Multicast + if (data_in = DEFAULT_IPv4_MULTICAST_ADDRESS) then + is_multicast_next <= '1'; + end if; + -- Checksum Calculation + add_a <= data_in; + add_b <= add_res; + add_cin <= add_cout; + -- Next Stage + stage_next <= UDP_HEADER_1; + -- packet not for us, skip + else + stage_next <= SKIP_PACKET; + end if; + else + -- Hold Checksum Value + add_a <= add_res; + end if; + when LEN => + -- Reset packet Byte Counter + reset_read_cnt <= '1'; + -- Hold Checksum Value + add_a <= add_res; + if (empty = '0') then + rd_sig <= '1'; + -- Read Packet Length from input + packet_length_next <= unsigned(data_in(13 downto 0)); + -- Check Packet Length + if(to_integer(unsigned(data_in(13 downto 0))) < MIN_PACKET_LENGTH) then + stage_next <= SKIP_PACKET; + else + -- Begin Processing + stage_next <= UDP_HEADER_1; + end if; + end if; + -- First UDP Header word (Fields: Src Port, Dest Port) + when UDP_HEADER_1 => + if (empty = '0') then + rd_sig <= '1'; + -- Latch Src Port + src_port_next <= udp_src_port; + -- Checksum Calculation + add_a <= data_in; + add_b <= add_res; + add_cin <= add_cout; + -- Default Next Stage + stage_next <= SKIP_PACKET; + -- Check if Dest Port is valid, if not Skip Packet + if (is_multicast = '1') then + -- Check if Metatraffic (via Mutlicast) + tmp := match_domain_id(udp_dest_port, META_IPv4_MULTICAST_PORT); + if (tmp != MAX_ENDPOINTS) then + is_metatraffic_next <= '1'; + participant_id_next <= tmp; + stage_next <= UDP_HEADER_2; + -- Check if User Traffic (via Multicast) + else + tmp := match_domain_id(udp_dest_port, USER_IPv4_MULTICAST_PORT); + if (tmp != MAX_ENDPOINTS) then + stage_next <= UDP_HEADER_2; + participant_id_next <= tmp; + end if; + end if; + else + -- Check if Metatraffic (via Unicast) + tmp := match_domain_id(udp_dest_port, META_IPv4_UNICAST_PORT); + if (tmp != MAX_ENDPOINTS) then + is_metatraffic_next <= '1'; + participant_id_next <= tmp; + stage_next <= UDP_HEADER_2; + -- Check if User Traffic (via Unicast) + else + tmp := match_domain_id(udp_dest_port, USER_IPv4_UNICAST_PORT); + if (tmp != MAX_ENDPOINTS) then + stage_next <= UDP_HEADER_2; + participant_id_next <= tmp; + end if; + end if; + end if; + else + -- Hold Checksum Value + add_a <= add_res; + end if; + -- Second UDP Header Word (Fields: Length, Checksum) + when UDP_HEADER_2 => + if (empty = '0') then + rd_sig <= '1'; + -- If UPD header length does not match actual packet length, skip packet + if (normalize_length(udp_length) /= std_logic_vector(packet_length)) then + stage_next <= SKIP_PACKET; + else + -- Checksum Calculation + add_a <= (udp_length & x"0000"); + add_b <= add_res; + add_cin <= add_cout; + -- Next Stage + stage_next <= RTPS_HEADER_1; + end if; + else + -- Hold Checksum Value + add_a <= add_res; + end if; + -- First RTPS Header word (Fields: Protocolld) + when RTPS_HEADER_1 => + if (empty = '0') then + rd_sig <= '1'; + -- If underlying Protocol is not RTPS, skip packet + if(data_in /= PROTOCOLLD_RTPS) then + stage_next <= SKIP_PACKET; + -- Continue Parsing + else + -- Checksum Calculation + add_a <= data_in; + add_b <= add_res; + add_cin <= add_cout; + -- Next Stage + stage_next <= RTPS_HEADER_2; + end if; + else + -- Hold Checksum Value + add_a <= add_res; + end if; + -- Second RTPS Header word (Fields: Protocol Version, Vendor ID) + when RTPS_HEADER_2 => + if (empty = '0') then + rd_sig <= '1'; + -- If RTPS Protocol Major Version is not 2, skip packet + if(rtps_version(15 downto 8) /= PROTOCOLVERSION_2_4(15 downto 8)) then + stage_next <= SKIP_PACKET; + -- Continue Parsing + else + -- Checksum Calculation + add_a <= data_in; + add_b <= add_res; + add_cin <= add_cout; + -- Reset GP Counter + cnt_next <= 1; + -- Next Stage + stage_next <= RTPS_HEADER_3; + + end if; + else + -- Hold Checksum Value + add_a <= add_res; + end if; + -- Rest of RTPS Header (Fields: GUID Prefix) + when RTPS_HEADER_3 => + if (empty = '0') then + rd_sig <= '1'; + -- Sender GUID_Prefix + TODO <= data_in; + -- Checksum Calculation + add_a <= data_in; + add_b <= add_res; + add_cin <= add_cout; + if (cnt = GUIDPREFIX_WIDTH/32) then + -- Next Stage + stage_next <= RTPS_SUB_HEADER; + else + cnt_next <= cnt + 1; + end if; + else + -- Hold Checksum Value + add_a <= add_res; + end if; + -- NOTE: From here on, due to the nature of the RTPS Protocol, 32-bit word alignement + -- is not guaranteed, and has to be handled. + -- RTPS Submessage Header (Fields: Submessage ID, Flags, Submessage Length) + when RTPS_SUB_HEADER => + if (empty = '0') then + rd_sig <= '1'; + + case (rtps_sub_id) is + when SID_ACKNACK => + -- Ignore Submessage if Final Flag not set + if (rtps_acknack_final = '1') then + -- Next Stage + stage_next <= ACKNACK_1; + else + -- Skip Submessage + stage_next <= SKIP_SUB; + end if; + when SID_PAD => + when SID_HEARTBEAT => + when SID_GAP => + when SID_INFO_TS => --state + when SID_INFO_SRC => --state + when SID_INFO_REPLY_IP4 => + when SID_INFO_DST => --state + when SID_INFO_REPLY => --state + when SID_NACK_FRAG => + when SID_HEARTBEAT_FRAG => + when SID_DATA => + when SID_DATA_FRAG => + -- Unknown ID, skip submessage + when others => + stage_next <= SKIP_SUB; + end case; + -- Checksum Calculation + add_a <= data_in; + add_b <= add_res; + add_cin <= add_cout; + -- Store Submessage Length + -- TODO: The below conditional +1 adder should be solved with a carry-in + sub_length_next <= packet_length + unsigned(normalize_length(rtps_sub_length)); + -- Store Byte offset of next Header + offset_latch_next <= unsigned(align_offset) + unsigned(rtps_sub_length(1 downto 0)); + -- Next Stage + align_sig_next <= data_in(23 downto 0); + else + -- Hold Checksum Value + add_a <= add_res; + end if; + when ACKNACK_1 => + if (empty = '0') then + rd_sig <= '1'; + --TODO + -- Checksum Calculation + add_a <= data_in; + add_b <= add_res; + add_cin <= add_cout; + -- Next Stage + align_sig_next <= data_in(23 downto 0); + else + -- Hold Checksum Value + add_a <= add_res; + end if; + when SKIP_PACKET => + if (empty = '0') then + rd_sig <= '1'; + -- End of Packet + if (read_cnt = packet_length) then + -- Continue parsing next packet + stage_next <= SRC_ADDR; + end if; + end if; + when SKIP_SUB => + if (empty = '0') then + rd_sig <= '1'; + -- End of Packet (No further Submessages) + if (read_cnt = packet_length) then + -- Continue parsing next packet + stage_next <= SRC_ADDR; + -- End of Submessage + elsif (read_cnt = sub_length) then + -- Begin parsing of next submessage + stage_next <= RTPS_SUB_HEADER; + -- Fix alignement + align_offset <= offset_latch; + end if; + end if; + when others => + null; + end case; + end process; + + -- Process responsible for counting read words + -- This process uses the actual FIFO read signals to determine reads + word_counter_prc : process(clk, reset) + begin + if rising_edge(clk) then + -- Reset Read counter + if (reset = '1' or reset_read_cnt = '1') then + read_cnt <= to_unsigned(1, read_cnt'length); + -- Increment read counter each time rd is high + elsif (rd_sig = '1' or buffer_ren = '1') then + read_cnt <= read_cnt + to_unsigned(1, read_cnt'length); + end if; + end if; + end process; + +end architecture; \ No newline at end of file diff --git a/src/rtps_package.vhd b/src/rtps_package.vhd index ad25526..4fb3134 100644 --- a/src/rtps_package.vhd +++ b/src/rtps_package.vhd @@ -3,145 +3,292 @@ 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; + -- 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 (NUM_READERS+NUM_WRITERS-1 downto 0) of integer; + type ENDPOINT_WITH_KEY_TYPE is array (NUM_READERS+NUM_WRITERS-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"; -subtype BYTE is std_logic_vector(7 downto 0); - -type GUIDPREFIX_TYPE is array (11 downto 0) of BYTE; -constant GUIDPREFIX_UNKNOWN : GUIDPREFIX_TYPE := (others => (others => '0')); - -type ENTITYID_TYPE is array (3 downto 0) of BYTE; -constant ENTITYID_UNKNOWN : ENTITYID_TYPE := (others => (others => '0')); ---alias ENTITYKIND : BYTE is ENTITYID(0); ---alias ENTITYKIND_UPPER : std_logic_vector(1 downto 0) is ENTITYKIND(7 downto 6); ---FOLLOWING MAP TO ENTITYKIND_UPPER -constant USER_DEFINED_ENTITY_KIND : std_logic_vector(1 downto 0) := "00"; -constant BUILT_IN_ENTITY_KIND : std_logic_vector(1 downto 0) := "11"; -constant VENDOR_SPECIFIC_ENTITY_KIND: std_logic_vector(1 downto 0) := "01"; ---alias ENTITYKIND_LOWER : std_logic_vector(1 downto 0) is ENTITYKIND(5 downto 0); ---FOLLOWING MAP TO ENTITYKIND_LOWER -constant WRITER_WITH_KEY : std_logic_vector(5 downto 0) := "000010"; -constant WRITER_NO_KEY : std_logic_vector(5 downto 0) := "000011"; -constant READER_NO_KEY : std_logic_vector(5 downto 0) := "000100"; -constant READER_WITH_KEY : std_logic_vector(5 downto 0) := "000111"; -constant WRITER_GROUP : std_logic_vector(5 downto 0) := "001000"; -constant READER_GROUP : std_logic_vector(5 downto 0) := "001001"; --- -constant ENTITYID_PARTICIPANT : ENTITYID_TYPE := (x"00", x"00", x"01", x"c1"); -constant ENTITYID_SEDP_BUILTIN_TOPICS_ANNOUNCER : ENTITYID_TYPE := (x"00", x"00", x"02", x"c2"); -constant ENTITYID_SEDP_BUILTIN_TOPICS_DETECTOR : ENTITYID_TYPE := (x"00", x"00", x"02", x"c7"); -constant ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER : ENTITYID_TYPE := (x"00", x"00", x"03", x"c2"); -constant ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR : ENTITYID_TYPE := (x"00", x"00", x"03", x"c7"); -constant ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER : ENTITYID_TYPE := (x"00", x"00", x"04", x"c2"); -constant ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR : ENTITYID_TYPE := (x"00", x"00", x"04", x"c7"); -constant ENTITYID_SPDP_BUILTIN_PARTICIPANT_ANNOUNCER : ENTITYID_TYPE := (x"00", x"01", x"00", x"c2"); -constant ENTITYID_SPDP_BUILTIN_PARTICIPANT_DETECTOR : ENTITYID_TYPE := (x"00", x"01", x"00", x"c7"); -constant ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER: ENTITYID_TYPE := (x"00", x"02", x"00", x"c2"); -constant ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER: ENTITYID_TYPE := (x"00", x"02", x"00", x"c7"); - -type VENDORID_TYPE is array (1 downto 0) of BYTE; ---GUIDPREFIX(1 downto 0) <= VENDORID; -constant VENDORID_UNKNOWN : VENDORID_TYPE := (others => (others => '0')); - -type PROTOCOLVERSION_TYPE is array (1 downto 0) of BYTE; -constant PROTOCOLVERSION_2_4 : PROTOCOLVERSION_TYPE := (x"02", x"04"); - ---General BITMAP SET Constants -constant NUMBITS_WIDTH : integer := 32; -constant BITMAP_WIDTH : integer := 32; - -constant SEQUENCENUMBER_WIDTH : integer := 64; -type SEQUENCENUMBERSET_HEADER_TYPE is record { - bitmapBase : std_logic_vector(SEQUENCENUMBER_WIDTH-1 downto 0); - numBits : std_logic_vector(NUMBITS_WIDTH-1 downto 0); -}; - -subtype FRAGMENTNUMBER_WIDTH : integer := 32;; -type FRAGMENTNUMBERSET_HEADER_TYPE is record{ - bitmapBase : std_logic_vector(FRAGMENTNUMBER_WIDTH-1 downto 0); - numBits : std_logic_vector(NUMBITS_WIDTH-1 downto 0); -}; - ---Fixed Point Q32.32 -constant TIMESTAMP_WIDTH : integer := 64; - -constant LOCATOR_KIND : integer := 32; -constant LOCATOR_PORT : integer := 32; -constant LOCATOR_ADDR : integer := 128; -type LOCATOR_TYPE is record{ - kind : std_logic_vector(LOCATOR_KIND-1 downto 0); - port : std_logic_vector(LOCATOR_PORT-1 downto 0); - addr : std_logic_vector(LOCATOR_ADDR-1 downto 0); -}; -constant NUMLOCATOR_WIDTH : integer := 32; - -constant LOCATORUDPv4_PORT : integer := 32; -constant LOCATORUDPv4_ADDR : integer := 32; -type LOCATORUDPv4_TYPE is record{ - addr : std_logic_vector(LOCATORUDPv4_ADDR-1 downto 0); - port : std_logic_vector(LOCATORUDPv4_PORT-1 downto 0); -}; - - -constant PARAMETERID_WIDTH : integer := 16; -constant PARAMETERLENGTH_WIDTH : integer := 16; -type PARAMETER_HEADER_TYPE is record{ - pid : std_logic_vector(PARAMETERID_WIDTH-1 downto 0); - length : std_logic_vector(PARAMETERLENGTH_WIDTH-1 downto 0); -}; - -constant PID_PAD : std_logic_vector(PARAMETERID_WIDTH-1 downto 0) := (others => '0'); -constant PID_SENTINEL : std_logic_vector(PARAMETERID_WIDTH-1 downto 0) := std_logic_vector(unsigned(1,PARAMETERID_WIDTH)); - -constant COUNT_WIDTH : integer := 32; - ---TODO: GroupDigest? - -constant PROTOCOLLD_WIDTH : integer := 32; -type RTPS_HEADER_TYPE is record{ - protocol : std_logic_vector(PROTOCOLLD_WIDTH-1 downto 0); - version : PROTOCOLVERSION_TYPE; - vendorid : VENDORID_TYPE; - guidprefix : GUIDPREFIX_TYPE; -}; -constant PROTOCOLLD_RTPS : std_logic_vector(PROTOCOLLD_WIDTH-1 downto 0) := x"52545053"; - -constant SUBMESSAGE_ID_WIDTH : integer := 8; -constant SUBMESSAGE_FLAGS_WIDTH : integer := 8; -constant SUBMESSAGE_LENGTH_WIDTH : integer := 16; -type RTPS_SUBMESSAGE_HEADER_TYPE is record{ - id : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0); - flags : std_logic_vector(SUBMESSAGE_FLAGS_WIDTH-1 downto 0); - length : std_logic_vector(SUBMESSAGE_LENGTH_WIDTH-1 downto 0); -}; - -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"; ---TODO: Enum it? - -constant ENDIANNESS_FLAG_POS : integer := 0; ---ACKNACK -constant FINAL_FLAG_POS : integer := 1; ---DATA -constant INLINEQOS_FLAG_POS : integer := 1; -constant DATA_FLAG_POS : integer := 2; -constant KEY_FLAG_POS : integer := 3; -constant NONSTANDARDPAYLOAD_FLAG_POS: integer := 4; - - - + 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(NUM_READERS+NUM_WRITERS-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 (NUM_READERS+NUM_WRITERS-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 (NUM_READERS+NUM_WRITERS-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 NUM_READERS+NUM_WRITERS-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; diff --git a/src/test.vhd b/src/test.vhd new file mode 100644 index 0000000..aa0bcd8 --- /dev/null +++ b/src/test.vhd @@ -0,0 +1,46 @@ +library ieee; +use ieee.std_logic_1164.all; +use ieee.numeric_std.all; + +package rtps_package is + + -- Number of Domains + constant NUM_DOMAIN : integer := 1; + ----------------------------------------------------------------------------------------------------- + -- *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); + + constant DOMAIN_ID_WIDTH : integer := 32; + + + + type DOMAIN_ID_TYPE is array (NUM_DOMAIN-1 downto 0) of std_logic_vector(DOMAIN_ID_WIDTH-1 downto 0); + + function gen_domain_ids (user_id : USER_DOMAIN_ID_TYPE) return DOMAIN_ID_TYPE; + + + +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); + +end package body; diff --git a/syn/project_1.xpr b/syn/project_1.xpr index 18cbe54..3398a7a 100644 --- a/syn/project_1.xpr +++ b/syn/project_1.xpr @@ -86,7 +86,9 @@ @@ -106,8 +108,9 @@