1059 lines
53 KiB
VHDL
1059 lines
53 KiB
VHDL
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
|
|
generic (
|
|
DOMAIN_ID : integer := 0;
|
|
DOMAIN_TAG : std_logic_vector(0 to (256*8)-1) := (others => '0')
|
|
)
|
|
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
|
|
|
|
--*****COMPONENT DECLARATION******
|
|
|
|
--*****CONSTANT DECLARATION*****
|
|
|
|
--*****TYPE DECLARATION*****
|
|
-- FSM states. Explained below in detail
|
|
type STAGE_TYPE is (TODO);
|
|
type MEM_STAGE_TYPE is (TODO);
|
|
type MESSAGE_TYPE_TYPE is (PDP, EDP, MESSAGE);
|
|
type MEM_OPCODE_TYPE is (NOP, SEARCH_PARTICIPANT, SEARCH_ENDPOINT);
|
|
|
|
|
|
--*****SIGNAL DECLARATION*****
|
|
-- FSM state
|
|
signal stage, stage_next : STAGE_TYPE := SRC_ADDR_HEADER;
|
|
signal mem_stage, mem_stage_next : MEM_STAGE_TYPE := TODO;
|
|
-- 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 Word aligned total packet length
|
|
signal packet_length, packet_length_next : unsigned(13 downto 0) := (others => '0');
|
|
signal opcode, opcode_next : std_logic_vector(7 downto 0) := (others => '0');
|
|
signal flags, flags_next : std_logic_vector(7 downto 0) := (others => '0');
|
|
signal src_port, src_port_next : std_logic_vector(15 downto 0) := (others => '0');
|
|
signal src_addr, src_addr_next : std_logic_vector(31 downto 0) := (others => '0');
|
|
signal addr_latch_1, addr_latch_1_next, addr_latch_2, addr_latch_2_next : std_logic_vector(31 downto 0) := (others => '0');
|
|
signal port_latch_1, port_latch_1_next, port_latch_2, port_latch_2_next : std_logic_vector(15 downto 0) := (others => '0');
|
|
signal src_entityid, src_entityid_next : std_logic_vector(31 downto 0) := (others => '0');
|
|
signal dest_entityid, dest_entityid_next : std_logic_vector(31 downto 0) := (others => '0');
|
|
signal parameter_end, parameter_end_next : unsigned(13 downto 0) := (others => '0');
|
|
signal message_type, message_type_next : MESSAGE_TYPE_TYPE := PDP;
|
|
signal data_in_swapped : std_logic_vector(31 downto 0) := (others => '0');
|
|
signal string_length, string_length_next : unsigned(31 downto 0) := (others => '0');
|
|
--TODO: Always reset before COMPARE_STRING stage
|
|
signal compare_length, compare_length_next : unsigned(31 downto 0) := (others => '0');
|
|
signal endpoint_match, endpoint_match_next : std_logic_vector(MAX_ENDPOINTS-1 downto 0) := (others => '0');
|
|
signal is_requested, is_requested_next : std_logic := '0';
|
|
signal lease_duration, lease_duration_next : DURATION_TYPE := (others => (others => '0'));
|
|
signal lifespan_duration, lifespan_duration_next : DURATION_TYPE := (others => (others => '0'));
|
|
signal addr_latch_index, addr_latch_index_next : std_logic := '0';
|
|
-- General Purpose counter
|
|
-- TODO: Constrain range
|
|
signal cnt, cnt_next : integer := 0;
|
|
signal expects_inline_qos, expects_inline_qos_next : std_logic := '0';
|
|
signal guid, guid_next : GUID_ARRAY_TYPE := (others => (others => '0'));
|
|
signal start_mem_op, mem_op_busy, mem_op_done : std_logic := '0';
|
|
signal mem_opcode : MEM_OPCODE_TYPE := NOP;
|
|
signal participant_message_best_effort, participant_message_best_effort_next : std_logic := '0';
|
|
|
|
--*****ALIAS DEFINATION*****
|
|
alias header_opcode : std_logic_vector(7 downto 0) is data_in(31 downto 24);
|
|
alias header_flags : std_logic_vector(7 downto 0) is data_in(23 downto 16);
|
|
alias header_udp_port : std_logic_vector(15 downto 0) is data_in(15 downto 0);
|
|
alias parameter_id : std_logic_vector(15 downto 0) is data_in(31 downto 16);
|
|
alias parameter_length : std_logic_vector(15 downto 0) is data_in(15 downto 0);
|
|
alias representation_id : std_logic_vector(15 downto 0) is data_in(31 downto 16);
|
|
alias representation_options : std_logic_vector(15 downto 0) is data_in(15 downto 0);
|
|
-- RTPS SUBMESSAGE FLAGS
|
|
alias endian_flag : std_logic is flags(0);
|
|
alias endian_flag_next : std_logic is flags_next(0);
|
|
alias qos_flag : std_logic is flags(1);
|
|
alias qos_flag_next : std_logic is flags_next(1);
|
|
alias data_flag : std_logic is flags(2);
|
|
alias key_flag : std_logic is flags(3);
|
|
alias payload_flag : std_logic is header_flags(4);
|
|
alias must_undersand : std_logic is parameter_id(14);
|
|
|
|
--*****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;
|
|
|
|
endian_swap_prc: process(all)
|
|
begin
|
|
data_in_swapped <= data_in;
|
|
if (endian_flag = '1') then
|
|
for i in 0 to 3 loop
|
|
data_in_swapped(i*8+8-1 downto i*8) <= data((3-i)*8+8-1 downto (3-i)*8);
|
|
end loop;
|
|
end if;
|
|
end process;
|
|
|
|
parse_prc: process(all)
|
|
variable tmp_mask : std_logic_vector(31 downto 0):= (others => '0');
|
|
begin
|
|
--DEFAULT
|
|
stage_next <= stage;
|
|
packet_length_next <= packet_length;
|
|
rd_sig <= '0';
|
|
reset_read_cnt <= '0';
|
|
opcode_next <= opcode;
|
|
flags_next <= flags;
|
|
src_port_next <= src_port;
|
|
src_addr_next <= src_addr;
|
|
src_entityid_next <= src_entityid;
|
|
dest_entityid_next <= dest_entityid;
|
|
parameter_end_next <= parameter_end;
|
|
message_type_next <= message_type;
|
|
string_length_next <= string_length;
|
|
endpoint_match_next <= endpoint_match;
|
|
is_requested_next <= is_requested;
|
|
lease_duration_next <= lease_duration;
|
|
lifespan_duration_next <= lifespan_duration;
|
|
addr_latch_1_next <= addr_latch_1;
|
|
addr_latch_2_next <= addr_latch_2;
|
|
port_latch_1_next <= port_latch_1;
|
|
port_latch_2_next <= port_latch_2;
|
|
addr_latch_index_next <= addr_latch_index;
|
|
cnt_next <= cnt;
|
|
expects_inline_qos_next <= expects_inline_qos;
|
|
mem_opcode <= NOP;
|
|
start_mem_op <= '0';
|
|
participant_message_best_effort_next <= participant_message_best_effort;
|
|
|
|
-- TODO: Reset Latches
|
|
|
|
case(stage) is
|
|
-- Initial/Idle State
|
|
when INIT =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
-- Latch Packet Length
|
|
packet_length_next <= unsigned(data_in(13 downto 0));
|
|
-- Reset Word Counter
|
|
reset_read_cnt <= '1';
|
|
-- Next Stage
|
|
stage_next <= PACKET_HEADER;
|
|
end if;
|
|
when PACKET_HEADER =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
-- Latch Opcode
|
|
opcode_next <= header_opcode;
|
|
-- Latch Flags
|
|
flags_next <= header_flags;
|
|
-- Latch Source UDP Port
|
|
src_port_next <= header_udp_port;
|
|
-- Next Stage
|
|
stage_next <= PACKET_SRC_ADDR;
|
|
-- SANITY CHECK
|
|
-- Skip Packet if non-standard Payload
|
|
if(header_opcode = SID_DATA and payload_flag = '1') then
|
|
stage_next <= SKIP_PACKET;
|
|
end if;
|
|
end if;
|
|
when PACKET_SRC_ADDR =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
-- Latch Source IP Address
|
|
src_addr_next <= data_in;
|
|
-- Next Stage
|
|
stage_next <= PACKET_SRC_ENTITYID;
|
|
end if;
|
|
when PACKET_SRC_ENTITYID =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
-- Latch Source Entity ID
|
|
src_entityid_next <= data_in;
|
|
-- Next Stage
|
|
stage_next <= PACKET_DEST_ENTITYID;
|
|
end if;
|
|
when PACKET_DEST_ENTITYID =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
|
|
-- DEFAULT STAGE
|
|
stage_next <= SKIP_PACKET;
|
|
|
|
-- *Check Dest Entity ID*
|
|
case (data_in) is
|
|
when ENTITYID_UNKNOWN =>
|
|
-- Wildcard Destination
|
|
-- Content has to be determined through src
|
|
stage_next <= CHECK_SRC_ENTITYID;
|
|
when ENTITYID_SPDP_BUILTIN_PARTICIPANT_DETECTOR =>
|
|
-- Only DATA relevant
|
|
if (opcode = SID_DATA) then
|
|
-- Packet Contains Participant Data
|
|
message_type_next <= PDP;
|
|
stage_next <= PROCESS_DATA;
|
|
end if;
|
|
when ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER =>
|
|
-- SANITY CHECK: Ignore if no Writers
|
|
-- Only ACKNACKs are relevant
|
|
if (NUM_WRITERS /= 0 and opcode = SID_ACKNACK) then
|
|
message_type_next <= EDP;
|
|
stage_next <= TODO; --ACKNACK Processing
|
|
end if;
|
|
when ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR =>
|
|
-- SANITY CHECK: Ignore if no Readers
|
|
if (NUM_READERS /= 0) then
|
|
-- Only HEARTBEATs and DATA are relevant (GAPs irrelevant, since depth is 1)
|
|
case (opcode) is
|
|
when SID_HEARTBEAT =>
|
|
message_type_next <= EDP;
|
|
stage_next <= TODO; -- HEARTBEAT Processing
|
|
when SID_DATA =>
|
|
message_type_next <= EDP;
|
|
stage_next <= PROCESS_DATA; -- DATA Processing
|
|
-- QoS is publisher-offered
|
|
is_requested_next <= '0';
|
|
-- Reset Endpoint Match (ALL READERS)
|
|
endpoint_match_next <= (others => '0');
|
|
endpoint_match_next(MAX_ENDPOINTS-1 downto MAX_ENDPOINTS-NUM_READERS) <= (others => '1');
|
|
end case;
|
|
end if;
|
|
when ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER =>
|
|
-- SANITY CHECK: Ignore if no Readers
|
|
-- Only ACKNACKs are relevant
|
|
if (NUM_READERS /= 0 and opcode = SID_ACKNACK) then
|
|
message_type_next <= EDP;
|
|
stage_next <= TODO; --ACKNACK Processing
|
|
end if;
|
|
when ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR =>
|
|
-- SANITY CHECK: Ignore if no Writers
|
|
if (NUM_WRITERS /= 0) then
|
|
-- Only HEARTBEATs and DATA are relevant (GAPs irrelevant, since depth is 1)
|
|
case (opcode) is
|
|
when SID_HEARTBEAT =>
|
|
message_type_next <= EDP;
|
|
stage_next <= TODO; -- HEARTBEAT Processing
|
|
when SID_DATA =>
|
|
message_type_next <= EDP;
|
|
stage_next <= PROCESS_DATA; -- DATA Processing
|
|
-- QoS is subscriber-requested
|
|
is_requested_next <= '1';
|
|
-- Reset Endpoint Match (ALL WRITERS)
|
|
endpoint_match_next <= (others => '0');
|
|
endpoint_match_next(NUM_WRITERS-1 downto 0) <= (others => '1');
|
|
end case;
|
|
end if;
|
|
when ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER =>
|
|
-- Only ACKNACKs are relevant
|
|
if (opcode = SID_ACKNACK) then
|
|
message_type_next <= MESSAGE;
|
|
stage_next <= TODO; --ACKNACK Processing
|
|
end if;
|
|
when ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER =>
|
|
-- Only HEARTBEATs and DATA are relevant (GAPs irrelevant, since depth is 1)
|
|
case (opcode) is
|
|
when SID_HEARTBEAT =>
|
|
message_type_next <= MESSAGE;
|
|
stage_next <= TODO; -- HEARTBEAT Processing
|
|
when SID_DATA =>
|
|
message_type_next <= MESSAGE;
|
|
stage_next <= PROCESS_DATA; -- DATA Processing
|
|
end case;
|
|
when others =>
|
|
null;
|
|
end case;
|
|
end if;
|
|
when CHECK_SRC_ENTITYID =>
|
|
-- DEFAULT STAGE
|
|
stage_next <= SKIP_PACKET;
|
|
|
|
-- *Check Dest Entity ID*
|
|
case (src_entityid) is
|
|
when ENTITYID_SPDP_BUILTIN_PARTICIPANT_ANNOUNCER =>
|
|
-- Only DATA relevant
|
|
if (opcode = SID_DATA) then
|
|
-- Packet Contains Participant Data
|
|
message_type_next <= PDP;
|
|
stage_next <= PROCESS_DATA;
|
|
end if;
|
|
when ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR =>
|
|
-- SANITY CHECK: Ignore if no Writers
|
|
-- Only ACKNACKs are relevant
|
|
if (NUM_WRITERS /= 0 and opcode = SID_ACKNACK) then
|
|
message_type_next <= EDP;
|
|
stage_next <= TODO; --ACKNACK Processing
|
|
end if;
|
|
when ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER =>
|
|
-- SANITY CHECK: Ignore if no Readers
|
|
if (NUM_READERS /= 0) then
|
|
-- Only HEARTBEATs and DATA are relevant (GAPs irrelevant, since depth is 1)
|
|
case (opcode) is
|
|
when SID_HEARTBEAT =>
|
|
message_type_next <= EDP;
|
|
stage_next <= TODO; -- HEARTBEAT Processing
|
|
when SID_DATA =>
|
|
message_type_next <= EDP;
|
|
stage_next <= PROCESS_DATA; -- DATA Processing
|
|
-- QoS is publisher-offered
|
|
is_requested_next <= '0';
|
|
-- Reset Endpoint Match (ALL READERS)
|
|
endpoint_match_next <= (others => '0');
|
|
endpoint_match_next(MAX_ENDPOINTS-1 downto MAX_ENDPOINTS-NUM_READERS) <= (others => '1');
|
|
end case;
|
|
end if;
|
|
when ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR =>
|
|
-- SANITY CHECK: Ignore if no Readers
|
|
-- Only ACKNACKs are relevant
|
|
if (NUM_READERS /= 0 and opcode = SID_ACKNACK) then
|
|
message_type_next <= EDP;
|
|
stage_next <= TODO; --ACKNACK Processing
|
|
end if;
|
|
when ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER =>
|
|
-- SANITY CHECK: Ignore if no Writers
|
|
if (NUM_WRITERS /= 0) then
|
|
-- Only HEARTBEATs and DATA are relevant (GAPs irrelevant, since depth is 1)
|
|
case (opcode) is
|
|
when SID_HEARTBEAT =>
|
|
message_type_next <= EDP;
|
|
stage_next <= TODO; -- HEARTBEAT Processing
|
|
when SID_DATA =>
|
|
message_type_next <= EDP;
|
|
stage_next <= PROCESS_DATA; -- DATA Processing
|
|
-- QoS is subscriber-requested
|
|
is_requested_next <= '1';
|
|
-- Reset Endpoint Match (ALL WRITERS)
|
|
endpoint_match_next <= (others => '0');
|
|
endpoint_match_next(NUM_WRITERS-1 downto 0) <= (others => '1');
|
|
end case;
|
|
end if;
|
|
when ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER =>
|
|
-- Only ACKNACKs are relevant
|
|
if (opcode = SID_ACKNACK) then
|
|
message_type_next <= MESSAGE;
|
|
stage_next <= TODO; --ACKNACK Processing
|
|
end if;
|
|
when ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER =>
|
|
-- Only HEARTBEATs and DATA are relevant (GAPs irrelevant, since depth is 1)
|
|
case (opcode) is
|
|
when SID_HEARTBEAT =>
|
|
message_type_next <= MESSAGE;
|
|
stage_next <= TODO; -- HEARTBEAT Processing
|
|
when SID_DATA =>
|
|
message_type_next <= MESSAGE;
|
|
stage_next <= PROCESS_DATA; -- DATA Processing
|
|
end case;
|
|
when others =>
|
|
null;
|
|
end case;
|
|
when PROCESS_DATA =>
|
|
-- Data Message contains inline QoS
|
|
if (qos_flag = '1') then
|
|
stage_next <= PROCESS_PL;
|
|
-- Data Message contains Payload
|
|
elsif (data_flag = '1') then
|
|
-- Process Payload Header
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
-- DEFAULT Stage Next
|
|
stage_next <= SKIP_PACKET;
|
|
|
|
-- Process Payload Header
|
|
case(representation_id) is
|
|
when PL_CDR_BE =>
|
|
endian_flag_next <= '0';
|
|
stage_next <= PROCESS_PL;
|
|
when PL_CDR_LE =>
|
|
endian_flag_next <= '1';
|
|
stage_next <= PROCESS_PL;
|
|
when CDR_BE =>
|
|
if (message_type = MESSAGE) then
|
|
endian_flag_next <= '0';
|
|
stage_next <= PROCESS_MESSAGE;
|
|
end if;
|
|
when CDR_LE =>
|
|
if (message_type = MESSAGE) then
|
|
endian_flag_next <= '1';
|
|
stage_next <= PROCESS_MESSAGE;
|
|
end if;
|
|
when others =>
|
|
null;
|
|
end case;
|
|
end if;
|
|
end if;
|
|
when PROCESS_PL =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
-- Latch Parameter End
|
|
parameter_end_next <= read_cnt + unsigned(normalize_length(endian_swap(endian_flag,parameter_length)));
|
|
-- DEFAULT
|
|
stage_next <= SKIP_PARAMETER;
|
|
-- TODO: Ignore all inline QoS?
|
|
|
|
case (parameter_id) is
|
|
when PID_TOPIC_NAME =>
|
|
-- Topic Name only relevant for EDP
|
|
if(qos_flag = '0' and message_type = EDP) then
|
|
stage_next <= LATCH_STRING_LENGTH;
|
|
end if;
|
|
when PID_TYPE_NAME =>
|
|
-- Type Name only relevant for EDP
|
|
if(qos_flag = '0' and message_type = EDP) then
|
|
stage_next <= LATCH_STRING_LENGTH;
|
|
end if;
|
|
when PID_USER_DATA =>
|
|
-- Ignore
|
|
null;
|
|
when PID_GROUP_DATA =>
|
|
-- Ignore
|
|
null;
|
|
when PID_TOPIC_DATA =>
|
|
-- Ignore
|
|
null;
|
|
when PID_DURABILITY =>
|
|
if(qos_flag = '0' and message_type = EDP) then
|
|
stage_next <= RXO_DURABILITY;
|
|
end if;
|
|
when PID_DURABILITY_SERVICE =>
|
|
-- Ignore
|
|
when PID_DEADLINE =>
|
|
if(qos_flag = '0' and message_type = EDP) then
|
|
stage_next <= RXO_DEADLINE_1;
|
|
end if;
|
|
when PID_LATENCY_BUDGET =>
|
|
-- Ignore
|
|
when PID_LIVELINESS =>
|
|
if(qos_flag = '0' and message_type = EDP) then
|
|
stage_next <= RXO_LIVELINESS;
|
|
end if;
|
|
when PID_RELIABILITY =>
|
|
if(qos_flag = '0' and message_type = EDP) then
|
|
stage_next <= RXO_RELIABILITY;
|
|
end if;
|
|
when PID_LIFESPAN =>
|
|
if(qos_flag = '0' and message_type = EDP) then
|
|
stage_next <= LATCH_LIFESPAN_1;
|
|
end if;
|
|
when PID_DESTINATION_ORDER =>
|
|
if(qos_flag = '0' and message_type = EDP) then
|
|
stage_next <= RXO_DESTINATION_ORDER;
|
|
end if;
|
|
when PID_HISTORY =>
|
|
-- Ignore
|
|
when PID_RESOURCE_LIMITS =>
|
|
-- Ignore
|
|
when PID_OWNERSHIP =>
|
|
if(qos_flag = '0' and message_type = EDP) then
|
|
stage_next <= RXO_OWNERSHIP;
|
|
end if;
|
|
when PID_OWNERSHIP_STRENGTH =>
|
|
-- Ignore
|
|
when PID_PRESENTATION =>
|
|
if(qos_flag = '0' and message_type = EDP) then
|
|
stage_next <= RXO_PRESENTATION;
|
|
end if;
|
|
when PID_PARTITION =>
|
|
if(qos_flag = '0' and message_type = EDP) then
|
|
stage_next <= RXO_PARTITION;
|
|
end if;
|
|
when PID_TIME_BASED_FILTER =>
|
|
-- Ignore
|
|
when PID_TRANSPORT_PRIORITY =>
|
|
-- Ignore
|
|
when PID_DOMAIN_ID =>
|
|
if(qos_flag = '0' and message_type = PDP) then
|
|
stage_next <= MATCH_DOMAIN_ID;
|
|
end if;
|
|
when PID_DOMAIN_TAG =>
|
|
if(qos_flag = '0' and message_type = PDP) then
|
|
stage_next <= LATCH_STRING_LENGTH;
|
|
end if;
|
|
when PID_PROTOCOL_VERSION =>
|
|
if(qos_flag = '0' and message_type = PDP) then
|
|
stage_next <= MATCH_PROTOCOL_VERSION;
|
|
end if;
|
|
when PID_VENDORID =>
|
|
-- Ignore
|
|
when PID_UNICAST_LOCATOR =>
|
|
if(qos_flag = '0' and message_type = EDP) then
|
|
stage_next <= LATCH_LOCATOR;
|
|
addr_latch_index_next <= '0';
|
|
-- Initialise counter
|
|
cnt_next <= 1;
|
|
end if;
|
|
when PID_MULTICAST_LOCATOR =>
|
|
if(qos_flag = '0' and message_type = EDP) then
|
|
stage_next <= LATCH_LOCATOR;
|
|
addr_latch_index_next <= '0';
|
|
-- Initialise counter
|
|
cnt_next <= 1;
|
|
end if;
|
|
when PID_DEFAULT_UNICAST_LOCATOR =>
|
|
if(qos_flag = '0' and message_type = DPD) then
|
|
stage_next <= LATCH_LOCATOR;
|
|
addr_latch_index_next <= '0';
|
|
-- Initialise counter
|
|
cnt_next <= 1;
|
|
end if;
|
|
when PID_DEFAULT_MULTICAST_LOCATOR =>
|
|
if(qos_flag = '0' and message_type = DPD) then
|
|
stage_next <= LATCH_LOCATOR;
|
|
addr_latch_index_next <= '0';
|
|
-- Initialise counter
|
|
cnt_next <= 1;
|
|
end if;
|
|
when PID_METATRAFFIC_UNICAST_LOCATOR =>
|
|
if(qos_flag = '0' and message_type = DPD) then
|
|
stage_next <= LATCH_LOCATOR;
|
|
addr_latch_index_next <= '1';
|
|
-- Initialise counter
|
|
cnt_next <= 1;
|
|
end if;
|
|
when PID_METATRAFFIC_MULTICAST_LOCATOR =>
|
|
if(qos_flag = '0' and message_type = DPD) then
|
|
stage_next <= LATCH_LOCATOR;
|
|
addr_latch_index_next <= '1';
|
|
-- Initialise counter
|
|
cnt_next <= 1;
|
|
end if;
|
|
when PID_EXPECTS_INLINE_QOS =>
|
|
stage_next <= LATCH_EXPECTS_INLINE_QOS;
|
|
when PID_PARTICIPANT_MANUAL_LIVELINESS_COUNT =>
|
|
-- Ignore
|
|
when PID_PARTICIPANT_LEASE_DURATION =>
|
|
if(qos_flag = '0' and message_type = DPD) then
|
|
stage_next <= LATCH_LEASE_DURATION_1;
|
|
end if;
|
|
when PID_CONTENT_FILTER_PROPERTY =>
|
|
-- Ignore
|
|
when PID_PARTICIPANT_GUID =>
|
|
if(qos_flag = '0' and message_type = DPD) then
|
|
stage_next <= LATCH_GUID;
|
|
-- Initialise counter
|
|
cnt_next <= 0;
|
|
end if;
|
|
when PID_GROUP_GUID =>
|
|
-- Ignore
|
|
when PID_BUILTIN_ENDPOINT_SET =>
|
|
if(qos_flag = '0' and message_type = DPD) then
|
|
stage_next <= CHECK_REMOTE_BUILTIN_ENDPOINTS;
|
|
end if;
|
|
when PID_BUILTIN_ENDPOINT_QOS =>
|
|
if(qos_flag = '0' and message_type = DPD) then
|
|
stage_next <= LATCH_BUILTIN_ENDPOINT_QOS;
|
|
end if;
|
|
when PID_PROPERTY_LIST =>
|
|
-- Ignore
|
|
when PID_TYPE_MAX_SIZE_SERIALIZED =>
|
|
-- Ignore
|
|
when PID_ENTITY_NAME =>
|
|
-- Ignore
|
|
when PID_ENDPOINT_GUID =>
|
|
if(qos_flag = '0' and message_type = EPD) then
|
|
stage_next <= LATCH_GUID;
|
|
-- Initialise counter
|
|
cnt_next <= 0;
|
|
end if;
|
|
when PID_CONTENT_FILTER_INFO =>
|
|
-- Ignore
|
|
when PID_COHERENT_SET =>
|
|
-- Ignore
|
|
when PID_DIRECTED_WRITE =>
|
|
-- Ignore
|
|
when PID_ORIGINAL_WRITER_INFO =>
|
|
-- Ignore
|
|
when PID_GROUP_COHERENT_SET =>
|
|
-- Ignore
|
|
when PID_GROUP_SEQ_NUM =>
|
|
-- Ignore
|
|
when PID_WRITER_GROUP_INFO =>
|
|
-- Ignore
|
|
when PID_SECURE_WRITER_GROUP_INFO =>
|
|
-- Ignore
|
|
when PID_KEY_HASH =>
|
|
-- Ignore
|
|
when PID_STATUS_INFO =>
|
|
-- Ignore
|
|
when PID_EXTENDED =>
|
|
-- TODO
|
|
when PID_PAD =>
|
|
-- Ignore
|
|
when PID_IGNORE =>
|
|
-- Ignore
|
|
when PID_SENTINEL =>
|
|
-- If Processing in-line QoS until now
|
|
if(qos_flag = '1') then
|
|
qos_flag_next <= '0';
|
|
stage_next <= PROCESS_DATA;
|
|
end if;
|
|
-- TODO: End Condition
|
|
when PID_LIST_END =>
|
|
-- TODO
|
|
when others =>
|
|
-- If MUST_UNDERSTAND Flag is set, we have incompatible communication. Drop Packet
|
|
if (must_undersand = '1') then
|
|
stage_next <= SKIP_PACKET;
|
|
-- Else skip Uknown Parameter
|
|
else
|
|
stage_next <= SKIP_PARAMETER;
|
|
end if;
|
|
end case;
|
|
end if;
|
|
when LATCH_STRING_LENGTH =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
string_length_next <= unsigned(data_in_swapped);
|
|
stage_next <= COMPARE_STRING;
|
|
compare_length_next <= (others => '0');
|
|
end if;
|
|
when COMPARE_STRING =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
-- Increment compare position counter
|
|
compare_length_next <= compare_length + to_unsigned(32, compare_length'length);
|
|
-- Compare Strings
|
|
if (message_type = EDP) then
|
|
for i in 0 to ENDPOINT_TOPIC'length-1 loop
|
|
if (data_in /= ENDPOINT_TOPIC(i)(to_integer(compare_length) to to_integer(compare_length)+7)) then
|
|
endpoint_match_next(i) <= '0';
|
|
end if;
|
|
end loop;
|
|
elsif (message_type = DPD) then
|
|
if (data_in /= DOMAIN_TAG(to_integer(compare_length) to to_integer(compare_length)+7)) then
|
|
endpoint_match_next(i) <= '0';
|
|
end if;
|
|
end if;
|
|
-- End of String (Exit Condition)
|
|
-- NOTE: This assumes that the Parameter is padded with zeroes
|
|
-- TODO: XTypes 7.4.1.1 requires padding bytes for "PLAIN_CDR" to be zero, but does not specify the value for "PL_CDR"
|
|
if (compare_length >= string_length) then
|
|
stage_next <= SKIP_PARAMETER;
|
|
end if;
|
|
end if;
|
|
when RXO_DURABILITY =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
|
|
-- Check QoS Compatibility
|
|
for i in 0 to ENDPOINT_TOPIC'length-1 loop
|
|
-- data-in is Requested
|
|
if (is_requested = '1') then
|
|
if (data_in_swapped > ENDPOINT_DURABILITY(i)) then
|
|
endpoint_match_next(i) <= '0';
|
|
end if;
|
|
-- data-in is Offered
|
|
else
|
|
if (data_in_swapped < ENDPOINT_DURABILITY(i)) then
|
|
endpoint_match_next(i) <= '0';
|
|
end if;
|
|
end if;
|
|
end loop;
|
|
-- Next Stage
|
|
stage_next <= SKIP_PARAMETER;
|
|
end if;
|
|
when RXO_DEADLINE_1 =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
|
|
-- Check QoS Compatibility
|
|
for i in 0 to ENDPOINT_TOPIC'length-1 loop
|
|
-- data-in is Requested
|
|
if (is_requested = '1') then
|
|
if (data_in_swapped < ENDPOINT_DEADLINE(i)(1)) then
|
|
endpoint_match_next(i) <= '0';
|
|
end if;
|
|
-- data-in is Offered
|
|
else
|
|
if (data_in_swapped > ENDPOINT_DEADLINE(i)(1)) then
|
|
endpoint_match_next(i) <= '0';
|
|
end if;
|
|
end if;
|
|
end loop;
|
|
-- Next Stage
|
|
stage_next <= RXO_DEADLINE_2;
|
|
end if;
|
|
when RXO_DEADLINE_2 =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
|
|
-- Check QoS Compatibility
|
|
for i in 0 to ENDPOINT_TOPIC'length-1 loop
|
|
-- data-in is Requested
|
|
if (is_requested = '1') then
|
|
if (data_in_swapped < ENDPOINT_DEADLINE(i)(0)) then
|
|
endpoint_match_next(i) <= '0';
|
|
end if;
|
|
-- data-in is Offered
|
|
else
|
|
if (data_in_swapped > ENDPOINT_DEADLINE(i)(0)) then
|
|
endpoint_match_next(i) <= '0';
|
|
end if;
|
|
end if;
|
|
end loop;
|
|
-- Next Stage
|
|
stage_next <= SKIP_PARAMETER;
|
|
end if;
|
|
when RXO_LIVELINESS =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
|
|
-- Check QoS Compatibility
|
|
for i in 0 to ENDPOINT_TOPIC'length-1 loop
|
|
-- data-in is Requested
|
|
if (is_requested = '1') then
|
|
if (data_in_swapped > ENDPOINT_LIVELINESS(i)) then
|
|
endpoint_match_next(i) <= '0';
|
|
end if;
|
|
-- data-in is Offered
|
|
else
|
|
if (data_in_swapped < ENDPOINT_LIVELINESS(i)) then
|
|
endpoint_match_next(i) <= '0';
|
|
end if;
|
|
end if;
|
|
end loop;
|
|
-- Next Stage
|
|
stage_next <= LATCH_LEASE_DURATION_1;
|
|
end if;
|
|
when LATCH_LEASE_DURATION_1 =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
-- Latch Lease Duration
|
|
lease_duration_next(1) <= data_in_swapped;
|
|
-- Next Stage
|
|
stage_next <= LATCH_LEASE_DURATION_2;
|
|
end if;
|
|
when LATCH_LEASE_DURATION_2 =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
-- Latch Lease Duration
|
|
lease_duration_next(0) <= data_in_swapped;
|
|
-- Next Stage
|
|
stage_next <= SKIP_PARAMETER;
|
|
end if;
|
|
when RXO_RELIABILITY =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
|
|
-- Check QoS Compatibility
|
|
for i in 0 to ENDPOINT_TOPIC'length-1 loop
|
|
-- data-in is Requested
|
|
if (is_requested = '1') then
|
|
if (data_in_swapped > ENDPOINT_RELIABILITY(i)) then
|
|
endpoint_match_next(i) <= '0';
|
|
end if;
|
|
-- data-in is Offered
|
|
else
|
|
if (data_in_swapped < ENDPOINT_RELIABILITY(i)) then
|
|
endpoint_match_next(i) <= '0';
|
|
end if;
|
|
end if;
|
|
end loop;
|
|
-- Next Stage
|
|
-- NOTE: The max_blocking_time value is ignored
|
|
stage_next <= SKIP_PARAMETER;
|
|
end if;
|
|
when LATCH_LIFESPAN_1 =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
-- Latch Lease Duration
|
|
lifespan_duration_next(1) <= data_in_swapped;
|
|
-- Next Stage
|
|
stage_next <= LATCH_LIFESPAN_2;
|
|
end if;
|
|
when LATCH_LIFESPAN_2 =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
-- Latch Lease Duration
|
|
lifespan_duration_next(0) <= data_in_swapped;
|
|
-- Next Stage
|
|
stage_next <= SKIP_PARAMETER;
|
|
end if;
|
|
when RXO_DESTINATION_ORDER =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
|
|
-- Check QoS Compatibility
|
|
for i in 0 to ENDPOINT_TOPIC'length-1 loop
|
|
-- data-in is Requested
|
|
if (is_requested = '1') then
|
|
if (data_in_swapped > ENDPOINT_DESTINATION_ORDER(i)) then
|
|
endpoint_match_next(i) <= '0';
|
|
end if;
|
|
-- data-in is Offered
|
|
else
|
|
if (data_in_swapped < ENDPOINT_DESTINATION_ORDER(i)) then
|
|
endpoint_match_next(i) <= '0';
|
|
end if;
|
|
end if;
|
|
end loop;
|
|
-- Next Stage
|
|
stage_next <= SKIP_PARAMETER;
|
|
end if;
|
|
when RXO_OWNERSHIP =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
-- Check QoS Compatibility
|
|
if (data_in_swapped /= (data_in_swapped'reverse_range => '0')) then
|
|
endpoint_match_next <= (others => '0');
|
|
end if;
|
|
-- Next Stage
|
|
stage_next <= SKIP_PARAMETER;
|
|
end if;
|
|
when RXO_PRESENTATION_1 =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
|
|
-- Check QoS Compatibility
|
|
for i in 0 to ENDPOINT_TOPIC'length-1 loop
|
|
-- data-in is Requested
|
|
if (is_requested = '1') then
|
|
if (data_in_swapped > ENDPOINT_PRESENTATION(i)) then
|
|
endpoint_match_next(i) <= '0';
|
|
end if;
|
|
-- data-in is Offered
|
|
else
|
|
if (data_in_swapped < ENDPOINT_PRESENTATION(i)) then
|
|
endpoint_match_next(i) <= '0';
|
|
end if;
|
|
end if;
|
|
end loop;
|
|
-- Next Stage
|
|
stage_next <= RXO_PRESENTATION_2;
|
|
end if;
|
|
when RXO_PRESENTATION_2 =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
|
|
-- Check QoS Compatibility
|
|
for i in 0 to ENDPOINT_TOPIC'length-1 loop
|
|
-- data-in is Requested
|
|
if (is_requested = '1') then
|
|
if (data_in(23) = '1' and ENDPOINT_COHERENT_ACCESS(i) = '0') then
|
|
endpoint_match_next(i) <= '0';
|
|
end if;
|
|
if (data_in(15) = '1' and ENDPOINT_ORDERED_ACCESS(i) = '0') then
|
|
endpoint_match_next(i) <= '0';
|
|
end if;
|
|
end if;
|
|
end loop;
|
|
-- Next Stage
|
|
stage_next <= SKIP_PARAMETER;
|
|
end if;
|
|
when RXO_PARTITION =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
|
|
--TODO: Implement full partition Name support?
|
|
-- NOTE: Endpoints are only matched against the default empty partition.
|
|
|
|
-- Check QoS Compatibility
|
|
if (data_in_swapped /= (data_in_swapped'reverse_range => '0')) then
|
|
endpoint_match_next <= (others => '0');
|
|
end if;
|
|
|
|
-- Next Stage
|
|
stage_next <= SKIP_PARAMETER;
|
|
end if;
|
|
when MATCH_DOMAIN_ID =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
-- DEAFULT Stage
|
|
stage_next <= SKIP_PARAMETER;
|
|
-- MATCH DOMAIN ID
|
|
if (data_in_swapped /= std_logic_vector(to_unsigned(DOMAIN_ID,data_in_swapped'length))) then
|
|
-- Drop Packet
|
|
stage_next <= SKIP_PACKET;
|
|
end if;
|
|
end if;
|
|
when MATCH_PROTOCOL_VERSION =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
-- DEAFULT Stage
|
|
stage_next <= SKIP_PARAMETER;
|
|
-- If RTPS Protocol Major Version is not 2, skip packet
|
|
if(data_in(31 downto 24) /= PROTOCOLVERSION_2_4(15 downto 8)) then
|
|
-- Drop Packet
|
|
stage_next <= SKIP_PACKET;
|
|
end if;
|
|
end if;
|
|
when LATCH_LOCATOR =>
|
|
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 (data_in_swapped /= LOCATOR_KIND_UDPv4) then
|
|
stage_next <= SKIP_PARAMETER;
|
|
end if;
|
|
-- Locator Port
|
|
elsif (cnt = 2) then
|
|
-- Latch Source Port
|
|
if (addr_latch_index = '0') then
|
|
port_latch_1 <= data_in_swapped(port_latch_1'length-1 downto 0);
|
|
else
|
|
port_latch_2 <= data_in_swapped(port_latch_2'length-1 downto 0);
|
|
end if;
|
|
-- Locator Addr (IPv4)
|
|
elsif (cnt = 6) then
|
|
-- Latch Src Addr
|
|
if (addr_latch_index = '0') then
|
|
addr_latch_1 <= data_in_swapped;
|
|
else
|
|
addr_latch_2 <= data_in_swapped;
|
|
end if;
|
|
-- Exit Stage
|
|
stage_next <= SKIP_PARAMETER;
|
|
end if;
|
|
end if;
|
|
when LATCH_EXPECTS_INLINE_QOS =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
|
|
-- Latch 'expectsInlineQoS'
|
|
expects_inline_qos_next <= data_in(24);
|
|
stage_next <= SKIP_PARAMETER;
|
|
end if;
|
|
when LATCH_GUID =>
|
|
-- NOTE: Checking here also for 'mem_op_done' prevents updating the GUID while the mem_op state machine could
|
|
-- potentially be using it.
|
|
if (empty = '0' and mem_op_done = '1') then
|
|
rd_sig <= '1';
|
|
-- Increment Counter (Default)
|
|
cnt_next <= cnt + 1;
|
|
|
|
-- Latch GUID
|
|
guid(cnt) <= data_in;
|
|
|
|
-- Exit Condition
|
|
if (cnt = 3) then
|
|
if (message_type = PDP) then
|
|
mem_opcode <= SEARCH_PARTICIPANT;
|
|
start_mem_op <= '1';
|
|
elsif (message_type = EDP) then
|
|
mem_opcode <= SEARCH_ENDPOINT;
|
|
start_mem_op <= '1';
|
|
end if;
|
|
stage_next <= SKIP_PARAMETER;
|
|
end if;
|
|
end if;
|
|
when CHECK_REMOTE_BUILTIN_ENDPOINTS =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
|
|
-- DEFAULT Next Stage
|
|
stage_next <= SKIP_PARAMETER;
|
|
|
|
-- SANITY CHECK (Builtin Participant and Participant Message Endpoints have to be always available)
|
|
if ( data_in_swapped(DISC_BUILTIN_ENDPOINT_PARTICIPANT_ANNOUNCER) = '0' or
|
|
data_in_swapped(DISC_BUILTIN_ENDPOINT_PARTICIPANT_DETECTOR) = '0' or
|
|
data_in_swapped(BUILTIN_ENDPOINT_PARTICIPANT_MESSAGE_DATA_WRITER) = '0' or
|
|
data_in_swapped(BUILTIN_ENDPOINT_PARTICIPANT_MESSAGE_DATA_READER) = '0') then
|
|
stage_next <= SKIP_PACKET;
|
|
end if;
|
|
-- Check for necessary Builtin Endpoints for Readers
|
|
if (NUM_READERS > 0) then
|
|
if ( data_in_swapped(DISC_BUILTIN_ENDPOINT_PUBLICATIONS_ANNOUNCER) = '0' or
|
|
data_in_swapped(DISC_BUILTIN_ENDPOINT_SUBSCRIPTIONS_DETECTOR) = '0') then
|
|
stage_next <= SKIP_PACKET;
|
|
end if;
|
|
end if;
|
|
-- Check for necessary Builtin Endpoints for Writers
|
|
if (NUM_WRITERS > 0) then
|
|
if ( data_in_swapped(DISC_BUILTIN_ENDPOINT_SUBSCRIPTIONS_ANNOUNCER) = '0' or
|
|
data_in_swapped(DISC_BUILTIN_ENDPOINT_PUBLICATIONS_DETECTOR) = '0') then
|
|
stage_next <= SKIP_PACKET;
|
|
end if;
|
|
end if;
|
|
end if;
|
|
when LATCH_BUILTIN_ENDPOINT_QOS =>
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
-- Latch QoS
|
|
participant_message_best_effort_next <= data_in_swapped(BEST_EFFORT_PARTICIPANT_MESSAGE_DATA_READER);
|
|
|
|
-- DEFAULT Next Stage
|
|
stage_next <= SKIP_PARAMETER;
|
|
end if;
|
|
--#############################
|
|
when SKIP_PARAMETER =>
|
|
-- End of Parameter
|
|
if (read_cnt > parameter_end) then
|
|
-- Begin parsing of next parameter
|
|
stage_next <= PROCESS_PL;
|
|
else
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
if (read_cnt = parameter_end) then
|
|
-- Begin parsing of next parameter
|
|
stage_next <= PROCESS_PL;
|
|
end if;
|
|
end if;
|
|
end if;
|
|
when SKIP_PACKET =>
|
|
-- TODO: Handle read_cnt > packet_length (Like above)
|
|
if (empty = '0') then
|
|
rd_sig <= '1';
|
|
-- End of Packet
|
|
if (read_cnt = packet_length) then
|
|
-- Continue parsing next packet
|
|
stage_next <= INIT;
|
|
end if;
|
|
end if;
|
|
when others =>
|
|
null;
|
|
end case;
|
|
end process;
|
|
|
|
mem_ctrl_prc : process(all)
|
|
begin
|
|
case (mem_stage) is
|
|
|
|
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;
|
|
|
|
end architecture;
|