70ace14c6b
- UDP Protocol * Added Synthesis Report for IPv4 Parser with different buffer sizes * Small fixes in IPv4 Handler * Added addsub Entity * Added Checksum entity * Implemented RTPS Parser - Compiles in Modelsim * Backup Version of RTPS Parser to extract and implement UDP Checksuming * Updated RTPS Package * Added VHDL comilation test file
516 lines
22 KiB
Plaintext
516 lines
22 KiB
Plaintext
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; |