From b8dc9e9482e9e0bc9098484ce2601d9767f23ef8 Mon Sep 17 00:00:00 2001 From: Greek64 Date: Sun, 4 Oct 2020 08:25:35 +0200 Subject: [PATCH] * Endoint Match/Unmatch --- src/TODO.txt | 142 +++++-- src/math_pkg.vhd | 13 +- src/rtps_builtin_endpoint.vhd | 772 ++++++++++++++++++++++++++++++---- src/rtps_handler.vhd | 108 +++-- src/rtps_package.vhd | 61 ++- src/single_port_ram.vhd | 4 +- 6 files changed, 921 insertions(+), 179 deletions(-) diff --git a/src/TODO.txt b/src/TODO.txt index 96be5bf..fc79147 100644 --- a/src/TODO.txt +++ b/src/TODO.txt @@ -73,6 +73,12 @@ ENDPOINT FIFO PACKET FORMAT +---------------------------------------------------------------+ | SRC_ENTITYID | +---------------------------------------------------------------+ +| | ++ + +| SRC_GUIDPREFIX | ++ + +| | ++---------------------------------------------------------------+ | DEST_ENTITYID [only for Builtin Destinations] | +---------------------------------------------------------------+ | | @@ -102,6 +108,104 @@ HEARTBEAT messages (and even ignore GAP messages). PARTICICPANT DATA ================= + 32..............24..............16..............8...............0 + +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + +---------------------------------------------------------------+ +01| | + + + +02| GUIDPREFIX | + + + +03| | + +---------------------------------------------------------------+ +04| META_IPv4_ADDRESS | + +---------------------------------------------------------------+ +05| DEFAULT_IPv4_ADDRESS | + +---------------------------------------------------------------+ +06| META_UDP_PORT | DEFAULT_UDP_PORT | + +---------------------------------------------------------------+ + | UNUSED | EXTRA_FLAGS |Q| + +---------------------------------------------------------------+ +07| LEASE_DURATION | + + + +08| | + +---------------------------------------------------------------+ +09| LEASE_DEADLINE | + + + +10| | + +---------------------------------------------------------------+ +12| | + + SPDP_SEQ_NR + +13| | + +---------------------------------------------------------------+ +14| | + + PUBLICATION_SEQ_NR + +15| | + +---------------------------------------------------------------+ +16| | + + SUBSCRIPTION_SEQ_NR + +17| | + +---------------------------------------------------------------+ +18| | + + MESSAGE_SEQ_NR + +19| | + +---------------------------------------------------------------+ + +ENDPOINT DATA +============= + 32..............24..............16..............8...............0 + +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + +---------------------------------------------------------------+ +01| ENTITYID | + +---------------------------------------------------------------+ +02| | + + + +03| GUIDPREFIX | + + + +04| | + +---------------------------------------------------------------+ +05| | + ~ ENDPOINT_BITMASK ~ +**| | + +---------------------------------------------------------------+ + +ENDPOINT MATCH FRAME +==================== + 32..............24..............16..............8...............0 + +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + +---------------------------------------------------------------+ +01| OPCODE | + +---------------------------------------------------------------+ +02| | + + + +03| GUIDPREFIX | + + + +04| | + +---------------------------------------------------------------+ +05| ENTITYID | + +---------------------------------------------------------------+ +06| IPv4_ADDRESS | + +---------------------------------------------------------------+ +07| UDP_PORT | EXTRA_FLAGS |Q| + +---------------------------------------------------------------+ + +ENDPOINT UNMATCH FRAME +====================== + 32..............24..............16..............8...............0 + +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + +---------------------------------------------------------------+ +01| OPCODE | + +---------------------------------------------------------------+ +02| | + + + +03| GUIDPREFIX | + + + +04| | + +---------------------------------------------------------------+ +05| ENTITYID | + +---------------------------------------------------------------+ + +LOCAL ENDPOINT BUFFER +===================== 32..............24..............16..............8...............0 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ +---------------------------------------------------------------+ @@ -111,42 +215,6 @@ PARTICICPANT DATA + + | | +---------------------------------------------------------------+ -| META_IPv4_ADDRESS | -+---------------------------------------------------------------+ -| DEFAULT_IPv4_ADDRESS | -+---------------------------------------------------------------+ -| META_UDP_PORT | DEFAULT_UDP_PORT | -+---------------------------------------------------------------+ -| LEASE_DURATION | -+ + -| | -+---------------------------------------------------------------+ -| LEASE_DEADLINE | -+ + -| | -+---------------------------------------------------------------+ -| EXTRA_FLAGS | -+---------------------------------------------------------------+ - - -32..............24..............16..............8...............0 -+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ -+---------------------------------------------------------------+ -| ENTITYID | -+---------------------------------------------------------------+ -| PARTICICPANT_INDEX | -+---------------------------------------------------------------+ -| | -~ ENDPOINT_BITMASK ~ -| | -+---------------------------------------------------------------+ - - -32..............24..............16..............8...............0 -+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ -+---------------------------------------------------------------+ -| PARTICICPANT_INDEX | -+---------------------------------------------------------------+ | ENTITYID | +---------------------------------------------------------------+ | IPv4_ADDRESS | @@ -158,6 +226,8 @@ PARTICICPANT DATA | (READER_ONLY) | +---------------------------------------------------------------+ + + TOPIC KEYS ========== Nominally the key is part of the serialized data of a data submessage. diff --git a/src/math_pkg.vhd b/src/math_pkg.vhd index 8b37393..9a749e3 100644 --- a/src/math_pkg.vhd +++ b/src/math_pkg.vhd @@ -55,4 +55,15 @@ package body math_pkg is end if; return ret_value; end function; - end package body; \ No newline at end of file + + function round_div(constant divident, divisor : in integer) return integer is + variable ret : integer; + begin + ret := divident / divisor; + if (divident mod divisor /= 0) then + ret := ret + divisor; + end if; + return ret; + end function; + + end package body; diff --git a/src/rtps_builtin_endpoint.vhd b/src/rtps_builtin_endpoint.vhd index 92c6cb8..3d38390 100644 --- a/src/rtps_builtin_endpoint.vhd +++ b/src/rtps_builtin_endpoint.vhd @@ -18,20 +18,41 @@ entity rtps_handler is 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 + endpoint_output : out USER_ENDPOINT_OUTPUT; + endpoint_full : in std_logic_vector(0 to MAX_ENDPOINTS-1); + endpoint_wr : out std_logic_vector(0 to MAX_ENDPOINTS-1); ); end entity; architecture arch of rtps_handler is --*****COMPONENT DECLARATION****** + component single_port_ram is + generic ( + ADDR_WIDTH : integer := 8; + DATA_WIDTH : integer := 12; + MEMORY_SIZE : integer := DATA_WIDTH*(2**ADDR_WIDTH) + + ); + port ( + clk : in std_logic; + addr : in std_logic_vector(ADDR_WIDTH-1 downto 0); + wen : in std_logic; + ren : in std_logic; + wr_data : in std_logic_vector(DATA_WIDTH-1 downto 0); + rd_data : out std_logic_vector(DATA_WIDTH-1 downto 0) + ); + end component; --*****CONSTANT DECLARATION***** + constant BUILTIN_BUFFER_SIZE : integer := MAX_REMOTE_PARTICIPANTS*PARTICIPANT_FRAME_SIZE; + constant BUILTIN_BUFFER_ADDR_WIDTH : integer := log2c(BUILTIN_BUFFER_SIZE); --*****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 MESSAGE_TYPE_TYPE is (PDP, EDP, MESSAGE, NONE); type MEM_OPCODE_TYPE is (NOP, SEARCH_PARTICIPANT, SEARCH_ENDPOINT); @@ -61,7 +82,11 @@ architecture arch of rtps_handler is 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 endpoint_mask, endpoint_mask_next : std_logic_vector(0 to MAX_ENDPOINTS-1) := (others => '0'); + signal endpoint_match, endpoint_match_next : std_logic_vector(0 to MAX_ENDPOINTS-1) := (others => '0'); + signal endpoint_unmatch, endpoint_unmatch_next : std_logic_vector(0 to MAX_ENDPOINTS-1) := (others => '0'); + signal endpoint_mask_array, endpoint_mask_array_next : ENDPOINT_BITMASK_ARRAY_TYPE := (others => (others => '0')); + signal participant_match, participant_match_next : std_logic := '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')); @@ -74,6 +99,25 @@ architecture arch of rtps_handler is 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'; + signal mem_addr, mem_addr_next, mem_addr_base, mem_addr_base_next : unsigned(BUILTIN_BUFFER_ADDR_WIDTH-1 downto 0) := (others => '0'); + signal addr_res, addr_res_next : unsigned(BUILTIN_BUFFER_ADDR_WIDTH-1 downto 0) := (others => '0'); + signal max_participant_addr, max_participant_addr_next : unsigned(BUILTIN_BUFFER_ADDR_WIDTH-1 downto 0) := (others => '0'); + signal max_endpoint_addr, max_endpoint_addr_next : unsigned(BUILTIN_BUFFER_ADDR_WIDTH-1 downto 0) := (others => '0'); + signal mem_read_data, mem_write_data : std_logic_vector(31 downto 0) := (others => '0'); + signal mem_rd, mem_wr, mem_busy, mem_done : std_logic := '0'; + -- TODO: Re-check range + signal mem_cnt, mem_cnt_next : integer range 0 to 3 := 0; + -- NOTE: Participant Index limited to 32 bits + signal participant_index, participant_index_next : std_logic_vector(31 downto 0) := (others => '0'); + signal seq_nr, seq_nr_next : std_logic_vector(SEQUENCE_NR_WIDTH-1 downto 0) := (others => '0'); + signal mem_seq_nr, mem_seq_nr_next : std_logic_vector(SEQUENCE_NR_WIDTH-1 downto 0) := (others => '0'); + signal is_orphan_search, is_orphan_search_next : std_logic := '0'; + signal orphan_entityid, orphan_entityid_next : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0'); + signal output_sig : std_logic_vector(31 downto 0) := (others => '0'); + signal mem_def_addr, mem_def_addr_next : std_logic_vector(31 downto 0) := (others => '0'); + signal mem_def_port, mem_def_port_next : std_logic_vector(15 downto 0) := (others => '0'); + signal mem_xflags, mem_xflags_next : std_logic_vector(15 downto 0) := (others => '0'); + --*****ALIAS DEFINATION***** alias header_opcode : std_logic_vector(7 downto 0) is data_in(31 downto 24); @@ -107,9 +151,52 @@ architecture arch of rtps_handler is return tmp; end function; + function convert_to_bitmask_array (bitmask : std_logic_vector) return ENDPOINT_BITMASK_ARRAY is + variable ret : ENDPOINT_BITMASK_ARRAY := (others => (others => '0')); + begin + ret := (others => (others => '0')); + for i in 0 to ENDPOINT_BITMASK_ARRAY'length-1 loop + if (i = ENDPOINT_BITMASK_ARRAY'length-1) then + ret(i) := bitmask(i*32 to i*32+31); + else + ret(i)(0 to (bitmask'length mod 32)-1) := bitmask(i*32 to i*32+(bitmask'length mod 32)-1); + end if; + end loop; + return ret; + end function; + + function convert_from_bitmask_array (bitmask : ENDPOINT_BITMASK_ARRAY, len : integer) return std_logic_vector is + variable ret : std_logic_vector(0 to len-1) := (others => '0'); + begin + ret := (others => '0'); + for i in 0 to ENDPOINT_BITMASK_ARRAY'length-1 loop + if (i = ENDPOINT_BITMASK_ARRAY'length-1) then + ret(i*32 to i*32+31) := bitmask(i); + else + ret(i*32 to i*32+(len mod 32)-1) := bitmask(i)(0 to (len mod 32)-1); + end if; + end loop; + return ret; + end function; begin + --*****COMPONENT INSTANTIATION***** + ram_inst : single_port_ram is + generic map ( + ADDR_WIDTH => BUILTIN_BUFFER_ADDR_WIDTH, + DATA_WIDTH => 32, + MEMORY_SIZE => BUILTIN_BUFFER_SIZE + ) + port map ( + clk => clk, + addr => mem_addr, + wen => mem_wr, + ren => mem_rd, + wr_data => mem_write_data, + rd_data => mem_read_data + ); + rd <= rd_sig; endian_swap_prc: process(all) @@ -122,8 +209,31 @@ begin end if; end process; + -- This process connects the Intermediate Output Signals to the actual output FIFOs + output_prc : process(all) + begin + -- Data Signal + for i in 0 to NUM_DOMAIN-1 loop + endpoint_output(i) <= output_sig; + end loop; + --Write Enable Signal + endpoint_wr <= (others => '0'); + case (stage) is + when INFORM_ENDPOINTS_MATCH => + if (wr_sig = '1') then + endpoint_wr <= endpoint_match; + end if; + when INFORM_ENDPOINTS_UNMATCH => + if (wr_sig = '1') then + endpoint_wr <= endpoint_unmatch; + end if; + when others => + null; + end case; + end process; + parse_prc: process(all) - variable tmp_mask : std_logic_vector(31 downto 0):= (others => '0'); + variable tmp_endpoint_mask : std_logic_vector(0 to MAX_ENDPOINTS-1) := (others => '0'); begin --DEFAULT stage_next <= stage; @@ -139,7 +249,10 @@ begin parameter_end_next <= parameter_end; message_type_next <= message_type; string_length_next <= string_length; + endpoint_mask_next <= endpoint_mask; endpoint_match_next <= endpoint_match; + endpoint_unmatch_next <= endpoint_unmatch; + participant_match_next <= participant_match_next; is_requested_next <= is_requested; lease_duration_next <= lease_duration; lifespan_duration_next <= lifespan_duration; @@ -152,7 +265,10 @@ begin expects_inline_qos_next <= expects_inline_qos; mem_opcode <= NOP; start_mem_op <= '0'; + is_orphan_search_next <= is_orphan_search; participant_message_best_effort_next <= participant_message_best_effort; + output_sig <= (others => '0'); + wr_sig <= '0'; -- TODO: Reset Latches @@ -198,8 +314,31 @@ begin rd_sig <= '1'; -- Latch Source Entity ID src_entityid_next <= data_in; + -- Reset Counter + cnt_next <= 1; -- Next Stage - stage_next <= PACKET_DEST_ENTITYID; + stage_next <= PACKET_SRC_GUIDPREFIX; + end if; + when PACKET_SRC_GUIDPREFIX => + if (empty = '0' and mem_op_done = '1') then + rd_sig <= '1'; + -- Latch Src GUIDPrefix + case (cnt) is + when 1 => + guid_next(0) <= data_in; + when 2 => + guid_next(1) <= data_in; + when 3 => + guid_next(2) <= data_in; + guid_next(3) <= (others => '0'); + -- Start Participant Search + mem_opcode <= SEARCH_PARTICIPANT; + start_mem_op <= '1'; + -- Next Stage + stage_next <= PACKET_DEST_ENTITYID; + when others => + null; + end case; end if; when PACKET_DEST_ENTITYID => if (empty = '0') then @@ -218,8 +357,10 @@ begin -- Only DATA relevant if (opcode = SID_DATA) then -- Packet Contains Participant Data - message_type_next <= PDP; - stage_next <= PROCESS_DATA; + message_type_next <= PDP; + stage_next <= LATCH_SEQ_NR; + -- Reset Participant Match + participant_match_next <= '1'; end if; when ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER => -- SANITY CHECK: Ignore if no Writers @@ -238,12 +379,12 @@ begin stage_next <= TODO; -- HEARTBEAT Processing when SID_DATA => message_type_next <= EDP; - stage_next <= PROCESS_DATA; -- DATA Processing + stage_next <= LATCH_SEQ_NR; -- 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'); + -- Reset Endpoint Mask (ALL READERS) + endpoint_mask_next <= (others => '0'); + endpoint_mask_next(MAX_ENDPOINTS-1 downto MAX_ENDPOINTS-NUM_READERS) <= (others => '1'); end case; end if; when ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER => @@ -263,12 +404,12 @@ begin stage_next <= TODO; -- HEARTBEAT Processing when SID_DATA => message_type_next <= EDP; - stage_next <= PROCESS_DATA; -- DATA Processing + stage_next <= LATCH_SEQ_NR; -- 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'); + -- Reset Endpoint Mask (ALL WRITERS) + endpoint_mask_next <= (others => '0'); + endpoint_mask_next(NUM_WRITERS-1 downto 0) <= (others => '1'); end case; end if; when ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER => @@ -285,7 +426,7 @@ begin stage_next <= TODO; -- HEARTBEAT Processing when SID_DATA => message_type_next <= MESSAGE; - stage_next <= PROCESS_DATA; -- DATA Processing + stage_next <= LATCH_SEQ_NR; -- DATA Processing end case; when others => null; @@ -302,7 +443,10 @@ begin if (opcode = SID_DATA) then -- Packet Contains Participant Data message_type_next <= PDP; - stage_next <= PROCESS_DATA; + stage_next <= LATCH_SEQ_NR; + -- Reset Participant Match + participant_match_next <= '1'; + end if; when ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR => -- SANITY CHECK: Ignore if no Writers @@ -321,12 +465,12 @@ begin stage_next <= TODO; -- HEARTBEAT Processing when SID_DATA => message_type_next <= EDP; - stage_next <= PROCESS_DATA; -- DATA Processing + stage_next <= LATCH_SEQ_NR; -- 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'); + -- Reset Endpoint Mask (ALL READERS) + endpoint_mask_next <= (others => '0'); + endpoint_mask_next(MAX_ENDPOINTS-1 downto MAX_ENDPOINTS-NUM_READERS) <= (others => '1'); end case; end if; when ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR => @@ -346,12 +490,12 @@ begin stage_next <= TODO; -- HEARTBEAT Processing when SID_DATA => message_type_next <= EDP; - stage_next <= PROCESS_DATA; -- DATA Processing + stage_next <= LATCH_SEQ_NR; -- 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'); + -- Reset Endpoint Mask (ALL WRITERS) + endpoint_mask_next <= (others => '0'); + endpoint_mask_next(NUM_WRITERS-1 downto 0) <= (others => '1'); end case; end if; when ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER => @@ -368,11 +512,25 @@ begin stage_next <= TODO; -- HEARTBEAT Processing when SID_DATA => message_type_next <= MESSAGE; - stage_next <= PROCESS_DATA; -- DATA Processing + stage_next <= LATCH_SEQ_NR; -- DATA Processing end case; when others => null; end case; + when LATCH_SEQ_NR => + if (empty = '0') then + rd_sig <= '1'; + -- Latch Sequence Number + case (cnt) is + when 1 => + seq_nr_next(63 downto 32) <= data_in; + when 2 => + seq_nr_next(31 downto 0) <= data_in; + stage_next <= PROCESS_DATA; + when others => + null; + end case; + end if; when PROCESS_DATA => -- Data Message contains inline QoS if (qos_flag = '1') then @@ -381,7 +539,7 @@ begin elsif (data_flag = '1') then -- Process Payload Header if (empty = '0') then - rd_sig <= '1'; + rd_sig <= '1'; -- DEFAULT Stage Next stage_next <= SKIP_PACKET; @@ -415,7 +573,9 @@ begin parameter_end_next <= read_cnt + unsigned(normalize_length(endian_swap(endian_flag,parameter_length))); -- DEFAULT stage_next <= SKIP_PARAMETER; - -- TODO: Ignore all inline QoS? + -- TODO: Use inline QoS? + -- TODO: DDS-XTYPES: When reading data, implementations of this specification shall be robust to any setting of the FLAG_MUST_UNDERSTAND bit and accept the parameter nevertheless. + case (parameter_id) is when PID_TOPIC_NAME => @@ -459,6 +619,7 @@ begin end if; when PID_LIFESPAN => if(qos_flag = '0' and message_type = EDP) then + -- TODO: Do not latch lifespan. Request it via inline QoS stage_next <= LATCH_LIFESPAN_1; end if; when PID_DESTINATION_ORDER => @@ -557,7 +718,7 @@ begin if(qos_flag = '0' and message_type = DPD) then stage_next <= LATCH_GUID; -- Initialise counter - cnt_next <= 0; + cnt_next <= 1; end if; when PID_GROUP_GUID => -- Ignore @@ -579,7 +740,7 @@ begin if(qos_flag = '0' and message_type = EPD) then stage_next <= LATCH_GUID; -- Initialise counter - cnt_next <= 0; + cnt_next <= 1; end if; when PID_CONTENT_FILTER_INFO => -- Ignore @@ -612,8 +773,9 @@ begin if(qos_flag = '1') then qos_flag_next <= '0'; stage_next <= PROCESS_DATA; + else + stage_next <= PARTICIPANT_MATCH_STAGE; end if; - -- TODO: End Condition when PID_LIST_END => -- TODO when others => @@ -642,12 +804,12 @@ begin 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'; + endpoint_mask_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'; + participant_match_next <= '0'; end if; end if; -- End of String (Exit Condition) @@ -666,12 +828,12 @@ begin -- data-in is Requested if (is_requested = '1') then if (data_in_swapped > ENDPOINT_DURABILITY(i)) then - endpoint_match_next(i) <= '0'; + endpoint_mask_next(i) <= '0'; end if; -- data-in is Offered else if (data_in_swapped < ENDPOINT_DURABILITY(i)) then - endpoint_match_next(i) <= '0'; + endpoint_mask_next(i) <= '0'; end if; end if; end loop; @@ -687,12 +849,12 @@ begin -- data-in is Requested if (is_requested = '1') then if (data_in_swapped < ENDPOINT_DEADLINE(i)(1)) then - endpoint_match_next(i) <= '0'; + endpoint_mask_next(i) <= '0'; end if; -- data-in is Offered else if (data_in_swapped > ENDPOINT_DEADLINE(i)(1)) then - endpoint_match_next(i) <= '0'; + endpoint_mask_next(i) <= '0'; end if; end if; end loop; @@ -708,12 +870,12 @@ begin -- data-in is Requested if (is_requested = '1') then if (data_in_swapped < ENDPOINT_DEADLINE(i)(0)) then - endpoint_match_next(i) <= '0'; + endpoint_mask_next(i) <= '0'; end if; -- data-in is Offered else if (data_in_swapped > ENDPOINT_DEADLINE(i)(0)) then - endpoint_match_next(i) <= '0'; + endpoint_mask_next(i) <= '0'; end if; end if; end loop; @@ -729,23 +891,50 @@ begin -- data-in is Requested if (is_requested = '1') then if (data_in_swapped > ENDPOINT_LIVELINESS(i)) then - endpoint_match_next(i) <= '0'; + endpoint_mask_next(i) <= '0'; end if; -- data-in is Offered else if (data_in_swapped < ENDPOINT_LIVELINESS(i)) then - endpoint_match_next(i) <= '0'; + endpoint_mask_next(i) <= '0'; end if; end if; end loop; -- Next Stage - stage_next <= LATCH_LEASE_DURATION_1; + stage_next <= RXO_LEASE_DURATION; + cnt_next <= 0; + end if; + when RXO_LEASE_DURATION => + -- TODO: Convert to two stages to avoid counter complexity? + 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_LEASE_DURATION(i)(cnt)) then + endpoint_mask_next(i) <= '0'; + end if; + -- data-in is Offered + else + if (data_in_swapped < ENDPOINT_LIVELINESS(i)(cnt)) then + endpoint_mask_next(i) <= '0'; + end if; + end if; + end loop; + --Increment Counter + cnt_next <= cnt + 1; + -- Exit Condition + if (cnt = 1) then + -- Next Stage + stage_next <= SKIP_PARAMETER; + end if; end if; when LATCH_LEASE_DURATION_1 => if (empty = '0') then rd_sig <= '1'; -- Latch Lease Duration - lease_duration_next(1) <= data_in_swapped; + lease_duration_next(0) <= data_in_swapped; -- Next Stage stage_next <= LATCH_LEASE_DURATION_2; end if; @@ -753,7 +942,7 @@ begin if (empty = '0') then rd_sig <= '1'; -- Latch Lease Duration - lease_duration_next(0) <= data_in_swapped; + lease_duration_next(1) <= data_in_swapped; -- Next Stage stage_next <= SKIP_PARAMETER; end if; @@ -766,12 +955,12 @@ begin -- data-in is Requested if (is_requested = '1') then if (data_in_swapped > ENDPOINT_RELIABILITY(i)) then - endpoint_match_next(i) <= '0'; + endpoint_mask_next(i) <= '0'; end if; -- data-in is Offered else if (data_in_swapped < ENDPOINT_RELIABILITY(i)) then - endpoint_match_next(i) <= '0'; + endpoint_mask_next(i) <= '0'; end if; end if; end loop; @@ -804,12 +993,12 @@ begin -- data-in is Requested if (is_requested = '1') then if (data_in_swapped > ENDPOINT_DESTINATION_ORDER(i)) then - endpoint_match_next(i) <= '0'; + endpoint_mask_next(i) <= '0'; end if; -- data-in is Offered else if (data_in_swapped < ENDPOINT_DESTINATION_ORDER(i)) then - endpoint_match_next(i) <= '0'; + endpoint_mask_next(i) <= '0'; end if; end if; end loop; @@ -821,7 +1010,7 @@ begin rd_sig <= '1'; -- Check QoS Compatibility if (data_in_swapped /= (data_in_swapped'reverse_range => '0')) then - endpoint_match_next <= (others => '0'); + endpoint_mask_next <= (others => '0'); end if; -- Next Stage stage_next <= SKIP_PARAMETER; @@ -835,12 +1024,12 @@ begin -- data-in is Requested if (is_requested = '1') then if (data_in_swapped > ENDPOINT_PRESENTATION(i)) then - endpoint_match_next(i) <= '0'; + endpoint_mask_next(i) <= '0'; end if; -- data-in is Offered else if (data_in_swapped < ENDPOINT_PRESENTATION(i)) then - endpoint_match_next(i) <= '0'; + endpoint_mask_next(i) <= '0'; end if; end if; end loop; @@ -856,10 +1045,10 @@ begin -- 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'; + endpoint_mask_next(i) <= '0'; end if; if (data_in(15) = '1' and ENDPOINT_ORDERED_ACCESS(i) = '0') then - endpoint_match_next(i) <= '0'; + endpoint_mask_next(i) <= '0'; end if; end if; end loop; @@ -875,7 +1064,7 @@ begin -- Check QoS Compatibility if (data_in_swapped /= (data_in_swapped'reverse_range => '0')) then - endpoint_match_next <= (others => '0'); + endpoint_mask_next <= (others => '0'); end if; -- Next Stage @@ -888,9 +1077,11 @@ begin 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; + -- No Match + participant_match_next <= '0'; end if; + -- Next Stage + stage_next <= SKIP_PARAMETER; end if; when MATCH_PROTOCOL_VERSION => if (empty = '0') then @@ -899,9 +1090,11 @@ begin 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; + -- No Match + participant_match_next <= '0'; end if; + -- Next Stage + stage_next <= SKIP_PARAMETER; end if; when LATCH_LOCATOR => if (empty = '0') then @@ -943,33 +1136,38 @@ begin 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 + if (empty = '0') 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; + -- Check if GUID Prefix valid (Should be the same as the GUID Prefix of the Packet) + -- and update the Entity ID + case (cnt) is + when 1 => + if (data_in /= guid(0)) then + stage_next <= SKIP_PACKET; + end if; + when 2 => + if (data_in /= guid(1)) then + stage_next <= SKIP_PACKET; + end if; + when 3 => + if (data_in /= guid(2)) then + stage_next <= SKIP_PACKET; + end if; + when 4 => + -- NOTE: Even though the mem_ctrl_prc is currently using the guid signal, it only uses the first + -- 3 bytes (GUIDPrefix) for the SEARCH_PARTICIPANT + guid_next(3) <= data_in; + stage_next <= SKIP_PARAMETER; + end case; end if; when CHECK_REMOTE_BUILTIN_ENDPOINTS => if (empty = '0') then rd_sig <= '1'; - -- DEFAULT Next Stage + -- Next Stage stage_next <= SKIP_PARAMETER; -- SANITY CHECK (Builtin Participant and Participant Message Endpoints have to be always available) @@ -977,20 +1175,23 @@ begin 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; + -- No Match + participant_match_next <= '0'; 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; + -- No Match + participant_match_next <= '0'; 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; + -- No Match + participant_match_next <= '0'; end if; end if; end if; @@ -1003,6 +1204,190 @@ begin -- DEFAULT Next Stage stage_next <= SKIP_PARAMETER; end if; + when PARTICIPANT_MATCH_STAGE => + -- Wait for Participant Search + if (mem_op_done = '1') then + -- No Match in Buffer + if (addr_res = to_unsigned(BUILTIN_BUFFER_SIZE, addr_res'length)) then + -- Participant Match + if (message_type = PDP and participant_match = '1') then + -- Add participant in buffer + mem_opcode <= INSERT_PARTICIPANT; + start_mem_op <= '1'; + -- DONE + stage_next <= SKIP_PACKET; + else + -- Ignore all other messages, since there is no corresponding participant in the buffer. + stage_next <= SKIP_PACKET; + end if; + -- Match in Buffer, New Sequence Number + elsif (mem_seq_nr > seq_nr) + -- Participant Unmatch + if (message_type = PDP and participant_match = '0') then + -- Remove participant from buffer + mem_opcode <= REMOVE_PARTICIPANT; + start_mem_op <= '1'; + -- Find and delete all orphaned endpoints in Buffer + stage_next <= FIND_ORPHAN_ENDPOINT; + is_orphan_search_next <= '1'; + -- Endpoint + elsif (message_type = EDP) then + -- Search Endpoint in Buffer + mem_opcode <= SEARCH_ENDPOINT; + start_mem_op <= '1'; + -- DONE + stage_next <= ENDPOINT_MATCH_STAGE; + else + -- Ignore + stage_next <= SKIP_PACKET; + end if; + else + -- Old Sequence Number, Ignore + stage_next <= SKIP_PACKET; + end if; + end if; + when ENDPOINT_MATCH_STAGE => + -- Wait for Endpoint Search + if (mem_op_done = '1') then + -- No Match in Buffer (New remote Endpoint) + if (addr_res = to_unsigned(BUILTIN_BUFFER_SIZE, addr_res'length)) then + -- At least one local endpoint match + if (endpoint_mask /= (endpoint_mask'range => '0')) then + -- Add endpoint in buffer + mem_opcode <= INSERT_ENDPOINT; + start_mem_op <= '1'; + -- Mark UNMATCHES + endpoint_match_next <= endpoint_mask; + endpoint_unmatch_next <= (others => '0'); + -- Propagate Match Changes to local Endpoints + stage_next <= INFORM_ENDPOINTS_MATCH; + cnt_next <= 1; + end if; + -- Match in buffer (Existing Endpoint) + else + -- At least one local endpoint match + if (endpoint_mask /= (endpoint_mask'range => '0')) then + tmp_endpoint_mask := convert_from_bitmask_array(endpoint_mask_array); + -- Mark Endpoint match changes + tmp_endpoint_mask := tmp_endpoint_mask xor endpoint_mask; + -- Mark UNMATCHES + endpoint_unmatch_next <= tmp_endpoint_mask xor endpoint_mask; + -- Mark NEW MATCHES + endpoint_match_next <= tmp_endpoint_mask and endpoint_mask; + -- Update endpoint in buffer + mem_opcode <= UPDATE_ENDPOINT; + start_mem_op <= '1'; + -- Propagate Match Changes to local Endpoints + stage_next <= INFORM_ENDPOINTS_MATCH; + cnt_next <= 1; + else + -- Remove endpoint from buffer + mem_opcode <= REMOVE_ENDPOINT; + start_mem_op <= '1'; + -- Mark UNMATCHES + endpoint_match_next <= (others => '0'); + endpoint_unmatch_next <= convert_from_bitmask_array(endpoint_mask_array); + -- Propagate Match Changes to local Endpoints + stage_next <= INFORM_ENDPOINTS_UNMATCH; + cnt_next <= 1; + end if; + end if; + end if; + when FIND_ORPHAN_ENDPOINT => + if (mem_op_done = '1') then + mem_opcode <= SEARCH_ENDPOINT; + start_mem_op <= '1'; + stage_next <= PURGE_ORPHAN_ENDPOINT; + end if; + when PURGE_ORPHAN_ENDPOINT => + if (mem_op_done = '1') then + -- Orphan Match + if (addr_res /= to_unsigned(BUILTIN_BUFFER_SIZE, addr_res'length)) then + -- Remove orphaned endpoint from buffer + mem_opcode <= REMOVE_ENDPOINT; + start_mem_op <= '1'; + -- Mark UNMATCHES + endpoint_match_next <= (others => '0'); + endpoint_unmatch_next <= convert_from_bitmask_array(endpoint_mask_array); + -- Propagate Match Changes to local Endpoints + stage_next <= INFORM_ENDPOINTS_UNMATCH; + cnt_next <= 1; + -- Buffer has no more orphans + else + -- DONE + stage_next <= SKIP_PACKET; + end if; + end if; + when INFORM_ENDPOINTS_MATCH => + if ((endpoint_match and endpoint_full) = (endpoint_full'range => '0')) then + -- Increment Counter + cnt_next <= cnt + 1; + -- Enable Write + wr_sig <= '1'; + case (cnt) is + when 1 => + output_sig <= OPCODE_MATCH; + when 2 => + output_sig <= guid(0); + when 3 => + output_sig <= guid(1); + when 4 => + output_sig <= guid(2); + when 5 => + output_sig <= guid(3); + when 6 => + -- Use Address set by PID + if (addr_latch_1 /= (addr_latch_1'reverse_range => '0')) then + output_sig <= addr_latch_1; + -- Else use the Default Address from Participant + else + output_sig <= mem_def_addr; + end if; + when 7 => + -- Use Port set by PID + if (port_latch_1 /= (port_latch_1'reverse_range => '0')) then + output_sig <= port_latch_1 & mem_xflags; + -- Else use the Default Port from Participant + else + output_sig <= mem_def_port & mem_xflags; + end if; + + -- If there are endpoints to unmatch, inform them + if (endpoint_unmatch /= (endpoint_unmatch'range => '0')) then + stage_next <= INFORM_ENDPOINTS_UNMATCH; + else + -- DONE + stage_next <= SKIP_PACKET; + end if; + end case; + end if; + when INFORM_ENDPOINTS_UNMATCH => + if ((endpoint_unmatch and endpoint_full) = (endpoint_full'range => '0')) then + -- Increment Counter + cnt_next <= cnt + 1; + -- Enable Write + wr_sig <= '1'; + case (cnt) is + when 1 => + output_sig <= OPCODE_UNMATCH; + when 2 => + output_sig <= guid(0); + when 3 => + output_sig <= guid(1); + when 4 => + output_sig <= guid(2); + when 5 => + output_sig <= guid(3); + + -- If we are in the middle of an orphan purge process, return to the search stage + if (is_orphan_search = '1') then + stage_next <= FIND_ORPHAN_ENDPOINT; + else + -- DONE + stage_next <= SKIP_PACKET; + end if; + end case; + end if; --############################# when SKIP_PARAMETER => -- End of Parameter @@ -1033,10 +1418,239 @@ begin end case; end process; + mem_op_busy <= not mem_op_done; + mem_ctrl_prc : process(all) + variable tmp : unsigned(mem_addr_base'range) := (others => '0'); begin - case (mem_stage) is + -- DEFAULT + mem_op_done <= '0'; + mem_stage_next <= mem_stage; + mem_addr_base_next <= mem_addr_base; + mem_addr_next <= mem_addr; + mem_rd <= '0'; + mem_wr <= '0'; + addr_res_next <= addr_res; + mem_cnt_next <= mem_cnt; + mem_seq_nr_next <= mem_seq_nr; + orphan_entityid_next <= orphan_entityid; + mem_def_addr_next <= mem_def_addr; + mem_def_port_next <= mem_def_port; + mem_xflags_next <= mem_xflags; + case (mem_stage) is + when IDLE => + mem_op_done <= '1'; + is_orphan_search_next <= '0'; + + if (start_mem_op = '1') then + case(mem_opcode) is + when SEARCH_PARTICIPANT => + mem_stage_next <= SEARCH_PARTICIPANT; + tmp := (others => '0'); + mem_addr_base_next <= tmp; + mem_addr_next <= tmp; + when SEARCH_ENDPOINT => + mem_stage_next <= SEARCH_ENDPOINT; + participant_index_next <= std_logic_vector(to_unsigned(BUILTIN_BUFFER_SIZE, participant_index'length)); + tmp := to_unsigned(BUILTIN_BUFFER_SIZE - ENDPOINT_FRAME_SIZE, mem_addr_base'length); + mem_addr_base_next <= tmp; + mem_addr_next <= tmp; + when others => + null; + end case; + end if; + when SEARCH_PARTICIPANT => + addr_res_next <= mem_addr_base; + mem_cnt_next <= 1; + mem_rd <= '1'; + mem_stage_next <= COMPARE_GUIDPREFIX; + -- Initial Address Increment + mem_addr_next <= mem_addr + to_unsigned(1, mem_addr'length); + -- Reached MAX Addr, No Match Found + if (mem_addr_base >= max_participant_addr) then + addr_res_next <= to_unsigned(BUILTIN_BUFFER_SIZE, addr_res'length); --No match + mem_stage_next <= IDLE; + end if; + when COMPARE_GUIDPREFIX => + mem_rd <= '1'; + -- Increment counter + mem_cnt_next <= mem_cnt + 1; + -- Default Address Increment + mem_addr_next <= mem_addr + to_unsigned(1, mem_addr'length); + -- TODO: Use this variant (Adder result in variable assignement) to denote that there is only one adder synthesized everywhere in the code? + -- Next Participant Frame Address + tmp := mem_addr_base + to_unsigned(PARTICIPANT_FRAME_SIZE, mem_addr_base'length); + case (mem_cnt) is + when 1 => + -- No Match + if (mem_read_data /= guid(0)) then + -- Continue Search + mem_stage_next <= SEARCH_PARTICIPANT; + mem_addr_next <= tmp; + mem_addr_base <= tmp; + end if; + when 2 => + -- No Match + if (mem_read_data /= guid(0)) then + -- Continue Search + mem_stage_next <= SEARCH_PARTICIPANT; + mem_addr_next <= tmp; + mem_addr_base <= tmp; + end if; + when 3 => + -- No Match + if (mem_read_data /= guid(0)) then + -- Continue Search + mem_stage_next <= SEARCH_PARTICIPANT; + mem_addr_next <= tmp; + mem_addr_base <= tmp; + -- Match Found + else + -- Get Default Endpoint Address + mem_stage_next <= GET_DEFAULT_ADDRESS; + mem_cnt_next <= 0; + mem_addr_next <= mem_addr_base + to_unsigned(4, mem_addr'length); + end if; + end case; + when GET_DEFAULT_ADDRESS => + mem_rd <= '1'; + -- Increment counter + mem_cnt_next <= mem_cnt + 1; + -- Default Address Increment + mem_addr_next <= mem_addr + to_unsigned(1, mem_addr'length); + case (mem_cnt) is + when 0 => + -- Memory Preload + null; + when 1 => + -- Latch IPv4 Address + mem_def_addr_next <= mem_read_data; + when 2 => + -- Latch UDPv4 Port + mem_def_port_next <= mem_read_data; + when 3 => + -- Latch Extra Flags + mem_xflags_next <= mem_read_data(15 downto 0); + + -- DEFAULT Next Stage + mem_stage_next <= GET_SEQ_NR; + mem_cnt_next <= 0; + -- NOTE: This assumes the message type has been correctly set by this time (Race Condition). + -- The mem_ctrl_prc needs at least 8 clk cycles before reaching this state, while the parse_prc takes + -- at most 3 clk cycles after issuing the mem_op before updating the message type. + case (message_type) is + when PDP => + mem_addr_next <= mem_addr_base + to_unsigned(11, mem_addr'length); + when EDP => + -- Publisher + if (is_requested = '0') then + mem_addr_next <= mem_addr_base + to_unsigned(13, mem_addr'length); + -- Subscriber + else + mem_addr_next <= mem_addr_base + to_unsigned(15, mem_addr'length); + end if; + when MESSAGE => + mem_addr_next <= mem_addr_base + to_unsigned(17, mem_addr'length); + when others => + -- Uknown Message Type. + mem_stage_next <= IDLE; + end case; + end case; + when GET_SEQ_NR => + mem_rd <= '1'; + -- Increment counter + mem_cnt_next <= mem_cnt + 1; + -- Default Address Increment + mem_addr_next <= mem_addr + to_unsigned(1, mem_addr'length); + case (mem_cnt) is + when 0 => + -- Memory Preload + null; + when 1 => + mem_seq_nr_next(63 downto 32) <= mem_read_data; + when 2 => + mem_seq_nr_next(31 downto 0) <= mem_read_data; + mem_stage_next <= IDLE; + end case; + when SEARCH_ENDPOINT => + addr_res_next <= mem_addr_base; + mem_addr_next <= mem_addr_base + to_unsigned(1, mem_addr'length); + mem_cnt_next <= 1; + mem_rd <= '1'; + mem_stage_next <= COMPARE_GUID; + + -- Reached MAX Addr, No Match Found + if (mem_addr_h <= max_endpoint_addr) then + addr_res_next <= to_unsigned(BUILTIN_BUFFER_SIZE, addr_res'length);; --No match + mem_stage_next <= IDLE; + end if; + when COMPARE_GUID => + mem_rd <= '1'; + -- Increment counter + mem_cnt_next <= mem_cnt + 1; + -- Default Address Increment + mem_addr_next <= mem_addr + to_unsigned(1, mem_addr'length); + -- Next Participant Frame Address + tmp := mem_addr_base - to_unsigned(ENDPOINT_FRAME_SIZE, mem_addr'length); + case (mem_cnt) is + when 1 => + -- If we search for orphan endpoint, ignore Entity id match and latch it for later use + if (is_orphan_search = '1') then + orphan_entityid_next <= mem_read_data; + else + -- No Match + -- NOTE: Endpoint GUID is stored with Entity ID first, and then the GUID Prefix + if (mem_read_data /= guid(3)) then + -- Continue Search + mem_stage_next <= SEARCH_ENDPOINT; + mem_addr_next <= tmp; + mem_addr_base <= tmp; + end if; + end if; + when 2 => + -- No Match + if (mem_read_data /= guid(0)) then + -- Continue Search + mem_stage_next <= SEARCH_ENDPOINT; + mem_addr_next <= tmp; + mem_addr_base <= tmp; + end if; + when 3 => + -- No Match + if (mem_read_data /= guid(1)) then + -- Continue Search + mem_stage_next <= SEARCH_ENDPOINT; + mem_addr_next <= tmp; + mem_addr_base <= tmp; + end if; + when 4 => + -- No Match + if (mem_read_data /= guid(2)) then + -- Continue Search + mem_stage_next <= SEARCH_ENDPOINT; + mem_addr_next <= tmp; + mem_addr_base <= tmp; + -- Match Found + else + mem_stage_next <= GET_ENDPOINT_MASK; + mem_cnt_next <= 0; + end if; + end case; + when GET_ENDPOINT_MASK => + mem_rd <= '1'; + -- Increment counter + mem_cnt_next <= mem_cnt + 1; + -- Default Address Increment + mem_addr_next <= mem_addr + to_unsigned(1, mem_addr'length); + -- Latch Endpoint Bitmask + endpoint_mask_array_next(mem_cnt) <= mem_read_data; + -- Exit Condition + if (mem_cnt = ENDPOINT_BITMASK_SIZE-1) then + -- DONE + mem_stage_next <= IDLE; + end if; + when others => end case; end process; diff --git a/src/rtps_handler.vhd b/src/rtps_handler.vhd index 8b7a65e..e5ff67a 100644 --- a/src/rtps_handler.vhd +++ b/src/rtps_handler.vhd @@ -6,6 +6,9 @@ use work.math_pkg.all; use work.rtps_package.all; -- TODO: Remove alignment logic for RTPS Submessages, since all Submessages are 32-bit aligned +-- TODO: Merge CHECK_SUB_END and SKIP_SUB stages +-- TODO: Fix payload length +-- TODO: Change all Endpoint Bit Vectors to "to" Ranges -- Checksum has to be checked before entity rtps_handler is @@ -111,6 +114,8 @@ architecture arch of rtps_handler is signal src_entityid, src_entityid_next : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0'); -- Destination Endpoint Entity ID latch signal dest_entityid, dest_entityid_next : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0'); + -- Source GUID Prefix latch + signal guidprefix, guidprefix_next : GUIDPREFIX_ARRAY_TYPE := (others => (others => '0')); -- Vector denoting the Destination User Endpoints of the Message signal user_endpoint, user_endpoint_next : std_logic_vector(MAX_ENDPOINTS-1 downto 0) := (others => '0'); -- Denoting if the Message is destined for the Built-in Endpoints @@ -133,6 +138,7 @@ architecture arch of rtps_handler is -- Denotes if Message is DATA Submessage signal is_data, is_data_next : std_logic := '0'; -- General Purpose counter + -- TODO: Recheck range signal cnt, cnt_next : integer range 0 to max(GUIDPREFIX_WIDTH/32, 6) := 0; --*****ALIAS DEFINATION***** @@ -348,6 +354,7 @@ begin domain_id_next <= domain_id; flags_next <= flags; src_entityid_next <= src_entityid; + guidprefix_next <= guidprefix; dest_entityid_next <= dest_entityid; user_endpoint_next <= user_endpoint; builtin_endpoint_next <= builtin_endpoint; @@ -501,13 +508,17 @@ begin when RTPS_HEADER_3 => if (empty = '0') then rd_sig <= '1'; - -- Sender GUID_Prefix - --TODO <= data_in; - if (cnt = GUIDPREFIX_WIDTH/32) then - -- Next Stage + -- Increment Counter + cnt_next <= cnt + 1; + + -- Latch Sender GUID_Prefix + if (cnt = 1) then + guidprefix_next(0) <= data_in; + elsif (cnt = 2) then + guidprefix_next(1) <= data_in; + elsif (cnt = 3) then + guidprefix_next(2) <= data_in; 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 @@ -748,34 +759,37 @@ begin 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; + case (cnt) is + -- Locator Kind + when 1 => + -- 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 + when 2 => + 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) + when 6 => + 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; + when others => + null; + end case; -- Latch Input for alignment purposes align_sig_next <= data_in(23 downto 0); end if; @@ -783,20 +797,26 @@ begin when INFO_SRC_HEADER => if (empty = '0') then rd_sig <= '1'; - -- Second Word of INFO_SRC Submessage + case (cnt) is + -- Second Word of INFO_SRC Submessage when 2 => - -- Default Next Stage - stage_next <= SKIP_SUB; -- Check Major Protocol Version if (aligned_data_in(31 downto 24) /= PROTOCOLVERSION_2_4(15 downto 8)) then -- Protocol not supported, skip rest of Packet stage_next <= SKIP_PACKET; end if; + when 3 => + guidprefix_next(0) <= aligned_data_in; + when 4 => + guidprefix_next(1) <= aligned_data_in; + when 5 => + guidprefix_next(2) <= aligned_data_in; + stage_next <= CHECK_SUB_END; when others => null; end case; - -- TODO: Check how many addres are synthesized for cnt/cnt_next + -- TODO: Check how many adders are synthesized for cnt/cnt_next cnt_next <= cnt + 1; -- Latch Input for alignment purposes align_sig_next <= data_in(23 downto 0); @@ -813,7 +833,6 @@ begin -- 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 @@ -924,6 +943,15 @@ begin when 4 => output_sig <= src_entityid; wr_sig <= '1'; + when 5 => + output_sig <= guidprefix(0); + wr_sig <= '1'; + when 6 => + output_sig <= guidprefix(1); + wr_sig <= '1'; + when 7 => + output_sig <= guidprefix(2); + wr_sig <= '1'; -- Next Stage if (builtin_endpoint = '1') then -- If the destination is a built-in Endpoint, push also the destination ID, @@ -1091,6 +1119,7 @@ begin domain_id <= 0; flags <= (others => '0'); src_entityid <= (others => '0'); + guidprefix <= (others => (others => '0')); dest_entityid <= (others => '0'); user_endpoint <= (others => '0'); numlocators <= (others => '0'); @@ -1118,6 +1147,7 @@ begin domain_id <= domain_id_next; flags <= flags_next; src_entityid <= src_entityid_next; + guidprefix <= guidprefix_next; dest_entityid <= dest_entityid_next; user_endpoint <= user_endpoint_next; numlocators <= numlocators_next; diff --git a/src/rtps_package.vhd b/src/rtps_package.vhd index 0a2038a..776bc62 100644 --- a/src/rtps_package.vhd +++ b/src/rtps_package.vhd @@ -2,6 +2,8 @@ library ieee; use ieee.std_logic_1164.all; use ieee.numeric_std.all; +use work.math_pkg.all; + package rtps_package is --*****USER CONFIG***** @@ -36,7 +38,7 @@ package rtps_package is 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; + type USER_DOMAIN_ID_TYPE is array (0 to NUM_DOMAIN-1) of integer; -- *DO NOT MODIFY END* ----------------------------------------------------------------------------------------------------- -- Array of Domain IDs @@ -44,14 +46,14 @@ package rtps_package is ----------------------------------------------------------------------------------------------------- -- *DO NOT MODIFY BEGIN* - type ENDPOINT_DOMAIN_MAP_TYPE is array (MAX_ENDPOINTS-1 downto 0) of integer; - type ENDPOINT_WITH_KEY_TYPE is array (MAX_ENDPOINTS-1 downto 0) of boolean; - type ENDPOINT_TOPIC_STRING_TYPE is array (MAX_ENDPOINTS-1 downto 0) of string(1 to 256); - type ENDPOINT_TOPIC_TYPE is array (MAX_ENDPOINTS-1 downto 0) of std_logic_vector(0 to (256*8)-1); - subtype QOS_TYPE is array (MAX_ENDPOINTS-1 downto 0) of std_logic_vector(31 downto 0); - subtype QOS_SLV_TYPE is array (MAX_ENDPOINTS-1 downto 0) of std_logic_vector(31 downto 0); - type DURATION_TYPE is array (1 downto 0) of unsigned(31 downto 0); - type ENDPOINT_DURATION_TYPE is array (MAX_ENDPOINTS-1 downto 0) of DURATION_TYPE; + type ENDPOINT_DOMAIN_MAP_TYPE is array (0 to MAX_ENDPOINTS-1) of integer; + type ENDPOINT_WITH_KEY_TYPE is array (0 to MAX_ENDPOINTS-1) of boolean; + type ENDPOINT_TOPIC_STRING_TYPE is array (0 to MAX_ENDPOINTS-1) of string(1 to 256); + type ENDPOINT_TOPIC_TYPE is array (0 to MAX_ENDPOINTS-1) of std_logic_vector(0 to (256*8)-1); + subtype QOS_TYPE is array (0 to MAX_ENDPOINTS-1) of std_logic_vector(31 downto 0); + subtype QOS_SLV_TYPE is array (0 to MAX_ENDPOINTS-1) of std_logic_vector(31 downto 0); + type DURATION_TYPE is array (0 to 1) of unsigned(31 downto 0); + type ENDPOINT_DURATION_TYPE is array (0 to MAX_ENDPOINTS-1) of DURATION_TYPE; constant DURATION_INFINITE : DURATION_TYPE := (x"7fffffff", x"ffffffff"); @@ -98,8 +100,8 @@ package rtps_package is constant ENDPOINT_TOPIC : ENDPOINT_TOPIC_TYPE; -- Deferred to Package Body constant ENDPOINT_DURABILITY : QOS_TYPE := (0 => VOLATILE_DURABILITY_QOS); constant ENDPOINT_PRESENTATION : QOS_TYPE := (0 => INSTANCE_PRESENTATION_QOS); - constant ENDPOINT_COHERENT_ACCESS : std_logic_vector(MAX_ENDPOINTS-1 downto 0) := (others => '0'); - constant ENDPOINT_ORDERED_ACCESS : std_logic_vector(MAX_ENDPOINTS-1 downto 0) := (others => '0'); + constant ENDPOINT_COHERENT_ACCESS : std_logic_vector(0 to MAX_ENDPOINTS-1) := (others => '0'); + constant ENDPOINT_ORDERED_ACCESS : std_logic_vector(0 to MAX_ENDPOINTS-1) := (others => '0'); constant ENDPOINT_DEADLINE : ENDPOINT_DURATION_TYPE := (0 => DURATION_INFINITE); --TODO: Assert constant ENDPOINT_LIVELINESS : QOS_TYPE := (0 => AUTOMATIC_LIVELINESS_QOS); constant ENDPOINT_LEASE_DURATION : ENDPOINT_DURATION_TYPE := (0 => DURATION_INFINITE); --TODO: Assert @@ -116,6 +118,10 @@ package rtps_package is constant ENDPOINT_MAX_INSTANCES : QOS_SLV_TYPE := (0 => std_logic_vector(to_unsigned(0,32))); --TODO: Assert constant ENDPOINT_MAX_SAMP_PER_INST : QOS_SLV_TYPE := (0 => std_logic_vector(to_unsigned(0,32))); --TODO: Assert + -- NOTE: The buffer will not only store participants, but also endpoint data + -- Used to determine the size of the built-inendpoint buffer + constant MAX_REMOTE_PARTICIPANTS : integer := 50; + --*****DDSI-RTPS 2.3***** -- Default Multicast Ipv4 Address (239.255.0.1) @@ -132,6 +138,7 @@ package rtps_package is constant PARAMETER_ID_WIDTH : integer := 16; constant PAYLOAD_REPRESENTATION_ID : integer := 16; constant PAYLOAD_REPRESENTATION_OPTIONS : integer := 16; + constant SEQUENCE_NR_WIDTH : integer := 64; -- 'RTPS' in Ascii code constant PROTOCOL_RTPS : std_logic_vector(PROTOCOL_WIDTH-1 downto 0) := x"52545053"; @@ -232,10 +239,10 @@ package rtps_package is - type DOMAIN_ID_TYPE is array (NUM_DOMAIN-1 downto 0) of std_logic_vector(DOMAIN_ID_WIDTH-1 downto 0); + type DOMAIN_ID_TYPE is array (0 to NUM_DOMAIN-1) of std_logic_vector(DOMAIN_ID_WIDTH-1 downto 0); constant DOMAIN_ID : DOMAIN_ID_TYPE; -- Deferred to Package Body - type DOMAIN_ENDPOINT_MAP_TYPE is array (NUM_DOMAIN-1 downto 0) of std_logic_vector(MAX_ENDPOINTS-1 downto 0); + type DOMAIN_ENDPOINT_MAP_TYPE is array (0 to NUM_DOMAIN-1) of std_logic_vector(MAX_ENDPOINTS-1 downto 0); constant DOMAIN_ENDPOINT_MAP : DOMAIN_ENDPOINT_MAP_TYPE; -- Deferred to Package Body -- Since this implementation runs on the same network stack and the RTPS Endpoints (Readers & Writers) @@ -244,17 +251,17 @@ package rtps_package is -- We generate just a 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); + type IPv4_PORT_TYPE is array (0 to NUM_DOMAIN-1) 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); - type GUIDPREFIX_ARRAY_TYPE is array (GUIDPREFIX_WIDTH/32-1 downto 0) of std_logic_vector(31 downto 0); - type GUID_ARRAY_TYPE is array ((GUIDPREFIX_WIDTH+ENTITYID_WIDTH)/32-1 downto 0) of std_logic_vector(31 downto 0); + type GUIDPREFIX_TYPE is array (0 to NUM_DOMAIN-1) of std_logic_vector(GUIDPREFIX_WIDTH-1 downto 0); + type GUIDPREFIX_ARRAY_TYPE is array (0 to GUIDPREFIX_WIDTH/32-1) of std_logic_vector(31 downto 0); + type GUID_ARRAY_TYPE is array (0 to (GUIDPREFIX_WIDTH+ENTITYID_WIDTH)/32-1) of std_logic_vector(31 downto 0); constant GUIDPREFIX : GUIDPREFIX_TYPE; -- Deferred to Package Body - constant GUIDPREFIX_UNKNOWN : std_logic_vector(GUIDPREFIX_WIDTH-1 downto 0) := (others => '0'); + constant GUIDPREFIX_UNKNOWN : std_logic_vector(0 to GUIDPREFIX_WIDTH-1) := (others => '0'); subtype ENTITY_KIND_H is std_logic_vector(1 downto 0); subtype ENTITY_KIND_L is std_logic_vector(5 downto 0); @@ -272,7 +279,7 @@ package rtps_package is -- DDSI-RTPS 2.3 states that Entity IDs have to be unique within each Participant. -- For simplicity and ease of mapping we make the Entity IDs unique across all Participant and Domains on this node. - type ENTITYID_TYPE is array (MAX_ENDPOINTS-1 downto 0) of std_logic_vector(ENTITYID_WIDTH-1 downto 0); + type ENTITYID_TYPE is array (0 to MAX_ENDPOINTS-1) 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"); @@ -309,11 +316,21 @@ package rtps_package is constant BEST_EFFORT_PARTICIPANT_MESSAGE_DATA_READER : integer := 0; --*****CUSTOM***** - + constant PARTICIPANT_FRAME_SIZE : integer := 12; + constant ENDPOINT_BITMASK_SIZE : integer := round_div(MAX_ENDPOINTS, 32); + constant ENDPOINT_FRAME_SIZE : integer := 2 + ENDPOINT_BITMASK_SIZE; + -- Limit Buffer to 32 bit Addresses + constant BUILTIN_BUFFER_SIZE : integer := min(MAX_ENDPOINTS*PARTICIPANT_FRAME_SIZE, 2**32); --**************** - type USER_ENDPOINT_OUTPUT is array (MAX_ENDPOINTS-1 downto 0) of std_logic_vector(31 downto 0); - type BUILTIN_ENDPOINT_TYPE is array (NUM_DOMAIN-1 downto 0) of std_logic_vector(31 downto 0); + constant ENDPOINT_MATCH_OPCODE_WIDTH: integer := 32; + + constant OPCODE_MATCH : std_logic_vector(ENDPOINT_MATCH_OPCODE_WIDTH-1 downto 0) := x"55000000"; + constant OPCODE_UNMATCH : std_logic_vector(ENDPOINT_MATCH_OPCODE_WIDTH-1 downto 0) := x"55000001"; + + type USER_ENDPOINT_OUTPUT is array (0 to MAX_ENDPOINTS-1) of std_logic_vector(31 downto 0); + type BUILTIN_ENDPOINT_TYPE is array (0 to NUM_DOMAIN-1) of std_logic_vector(31 downto 0); + type ENDPOINT_BITMASK_ARRAY_TYPE is array (0 to ENDPOINT_BITMASK_SIZE-1) of std_logic_vector(31 downto 0); end package; diff --git a/src/single_port_ram.vhd b/src/single_port_ram.vhd index 8dd05f9..7e98255 100644 --- a/src/single_port_ram.vhd +++ b/src/single_port_ram.vhd @@ -36,7 +36,7 @@ begin MEMORY_INIT_PARAM => "0", MEMORY_OPTIMIZATION => "true", MEMORY_PRIMITIVE => "auto", - MEMORY_SIZE => DATA_WIDTH*(2**ADDR_WIDTH), + MEMORY_SIZE => MEMORY_SIZE, MESSAGE_CONTROL => 0, READ_DATA_WIDTH_A => DATA_WIDTH, READ_LATENCY_A => 1, @@ -63,4 +63,4 @@ begin wea => (others => wen) --1-bit Vector ); -end architecture; \ No newline at end of file +end architecture;