diff --git a/src/rtps_config_package.vhd b/src/rtps_config_package.vhd index 30bb545..cb10c3d 100644 --- a/src/rtps_config_package.vhd +++ b/src/rtps_config_package.vhd @@ -49,6 +49,16 @@ package rtps_config_package is constant OPCODE_PARTICIPANT_UNMATCH : std_logic_vector(ENDPOINT_MATCH_OPCODE_WIDTH-1 downto 0) := x"55000002"; constant OPCODE_LIVELINESS_UPDATE : std_logic_vector(ENDPOINT_MATCH_OPCODE_WIDTH-1 downto 0) := x"55000003"; + type HISTORY_CACHE_OPCODE_TYPE is (NOP, ADD_CACHE_CHANGE); + type KEY_GENERATOR_OPCODE_TYPE is (NOP, WRITE_PAYLOAD, READ_KEY, READ_SIZE); + + -- Status Info Flags + constant DISPOSED_FLAG : natural := 0; + constant UNREGISTERED_FLAG : natural := 1; + constant FILTERED_FLAG : natural := 2; + constant PAYLOAD_FLAG : natural := 29; + constant KEY_HASH_FLAG : natural := 30; + constant READ_FLAG : natural := 31; -- Marks the Reader Endpoint in the Endpoint Array constant ENDPOINT_READERS : std_logic_vector(0 to NUM_ENDPOINTS-1) := (0 to NUM_READERS-1 => '1', others => '0'); diff --git a/src/rtps_endpoint.vhd b/src/rtps_endpoint.vhd new file mode 100644 index 0000000..682ca61 --- /dev/null +++ b/src/rtps_endpoint.vhd @@ -0,0 +1,1822 @@ +library ieee; +use ieee.std_logic_1164.all; +use ieee.numeric_std.all; + +use work.rtps_package.all; +use work.user_config.all; +use work.rtps_config_package.all; + +-- TODO: Use TIME_INVALID instead of zero. (Backport to builtin_endpoint) [Pay atention to (0) bit of ACKNACK and HEARTBEAT Deadlines, if compatible] +-- TODO: Check for special values of time/duration? +-- TODO: Remove last_word flag from metattraffic operations +-- TODO: Replace all ('range => 0) checks with defined constants, where possible +-- TODO: Remove highest_seq_nr in stand alone git commit + +entity rtps_endpoint is + generic ( + ID : natural; + ); + port ( + clk : in std_logic; + reset : in std_logic; + time : in TIME_TYPE; + alive : out std_logic; + + empty : in std_logic; + rd : out std_logic; + last_word_in : in std_logic; + data_in : in std_logic_vector(WORD_WIDTH-1 downto 0); + + meta_empty : in std_logic; + meta_data_in : in std_logic_vector(WORD_WIDTH-1 downto 0); + meta_rd : out std_logic; + + rtps_wr : out std_logic; + rtps_full : in std_logic; + last_word_out : out std_logic; + data_out : out std_logic_vector(WORD_WIDTH-1 downto 0); + + hc_start : out std_logic; + hc_opcode : out HISTORY_CACHE_OPCODE_TYPE; + hc_ack : in std_logic; + hc_data_out : in std_logic_vector(WORD_WIDTH-1 downto 0); + hc_valid_out : in std_logic; + hc_ready_out : out std_logic; + hc_last_word_out: in std_logic; + hc_data_in : out std_logic_vector(WORD_WIDTH-1 downto 0); + hc_valid_in : out std_logic; + hc_ready_in : in std_logic; + hc_last_word_in : out std_logic + ); +end entity; + +architecture arch of rtps_endpoint is + + --*****CONSTANT DECLARATION***** + -- Endpoint Memory Size in 4-Byte Words + constant ENDPOINT_MEMORY_SIZE : natural := TODO; + -- Endpoint Memory Address Width + constant ENDPOINT_MEMORY_WIDTH : natural := log2c(ENDPOINT_MEMORY_SIZE); + -- Highest Memory Address + constant MAX_ADDRESS : unsigned(ENDPOINT_MEMORY_WIDTH-1 downto 0) := to_unsigned(ENDPOINT_MEMORY_SIZE-1, ENDPOINT_MEMORY_WIDTH); + -- Highest Endpoint Frame Address + constant MAX_ENDPOINT_ADDRESS : unsigned(ENDPOINT_MEMORY_WIDTH-1 downto 0) := MAX_ADDRESS - ENDPOINT_FRAME_SIZE + 1; + -- Address pointing to the beginning of the first Endpoint Data Frame + constant FIRST_ENDPOINT_ADDRESS : unsigned(ENDPOINT_MEMORY_WIDTH-1 downto 0) := (others => '0'); + + -- *UPDATE PARTICIPANT FLAG POSITIONS* + constant UPDATE_ENDPOINT_FLAG_WIDTH : natural := 6; + -- Signifies that the main Endpoint Data are updated + constant ENDPOINT_DATA_FLAG : std_logic_vector(UPDATE_ENDPOINT_FLAG_WIDTH-1 downto 0) := (0 => 1, others => '0'); + -- Signifies that the Lease Deadline of the Endpoint Data is updated + constant LEASE_DEADLINE_FLAG : std_logic_vector(UPDATE_ENDPOINT_FLAG_WIDTH-1 downto 0) := (1 => 1, others => '0'); + -- Signifies that the last Sequence Number of the Endpoint Data is updated + constant NEXT_SEQ_NR_FLAG : std_logic_vector(UPDATE_ENDPOINT_FLAG_WIDTH-1 downto 0) := (2 => 1, others => '0'); + -- Signifies that the Highest Available Sequence Number of the Endpoint Data is updated + constant HIGHEST_SEQ_NR_FLAG : std_logic_vector(UPDATE_ENDPOINT_FLAG_WIDTH-1 downto 0) := (3 => 1, others => '0'); + -- Signifies that the HEARTBEAT/ACKNACK Timeout Time of the Endpoint Data is updated + constant RES_TIME_FLAG : std_logic_vector(UPDATE_ENDPOINT_FLAG_WIDTH-1 downto 0) := (4 => 1, others => '0'); + constant BITMAP_BLOCK_SIZE : natural := 16; + + --*****TYPE DECLARATION***** + -- FSM states. Explained below in detail + type STAGE_TYPE is (IDLE, TODO); + -- Memory FSM Opcodes + type MEM_OPCODE_TYPE is (NOP, TODO); + type MEM_STAGE_TYPE is (IDLE, TODO); + -- Record of all Participant Data stored in memory + type ENDPOINT_DATA_TYPE is record + addr : std_logic_vector(IPv4_ADDRESS_WIDTH-1 downto 0); + portn : std_logic_vector(UDP_PORT_WIDTH-1 downto 0); + expects_inline_qos : std_logic; + lease_deadline : TIME_TYPE; + res_time : TIME_TYPE; + next_seq_nr : SEQUENCENUMBER_TYPE; + highest_seq_nr : SEQUENCENUMBER_TYPE; + end record; + constant ZERO_ENDPOINT_DATA : ENDPOINT_DATA_TYPE := ( + addr => IPv4_ADDRESS_INVALID, + portn => UDP_PORT_INVALID, + expects_inline_qos => '0', + lease_deadline => TIME_INVALID, + res_time => TIME_INVALID, + next_seq_nr => SEQUENCENUMBER_UNKNOWN, + highest_seq_nr => SEQUENCENUMBER_UNKNOWN + ); + type MEM_CTRL_DATA_TYPE is record + guid : GUID_TYPE; + addr : std_logic_vector(IPv4_ADDRESS_WIDTH-1 downto 0); + portn : std_logic_vector(UDP_PORT_WIDTH-1 downto 0); + expects_inline_qos : std_logic; + deadline : TIME_TYPE; + next_seq_nr : SEQUENCENUMBER_TYPE; + highest_seq_nr : SEQUENCENUMBER_TYPE; + update_flags : std_logic_vector(UPDATE_ENDPOINT_FLAG_WIDTH-1 downto 0); + mem_opcode : MEM_OPCODE_TYPE; + end record; + constant ZERO_MEM_CTRL_DATA : MEM_CTRL_DATA_TYPE := ( + guid => (others => (others => '0')), + addr => (others => '0'), + portn => (others => '0'), + expects_inline_qos => '0', + deadline => TIME_INVALID, + next_seq_nr => SEQUENCENUMBER_UNKNOWN, + highest_seq_nr => SEQUENCENUMBER_UNKNOWN, + update_flags => (others => '0'), + mem_opcode => IDLE + ); + + + --*****SIGNAL DECLARATION***** + -- FSM state + signal stage, stage_next : STAGE_TYPE := IDLE; + signal meta_opcode, meta_opcode_next : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0'); + signal cnt, cnt_next : natural range TODO := 0; + signal guid, guid_next : GUID_TYPE := (others => (others => '0')); + signal mem_op_done : std_logic := '0'; + signal mem_opcode : MEM_OPCODE_TYPE := NOP; + signal mem_op_start : std_logic := '0'; + signal addr, addr_next : std_logic_vector(IPv4_ADDRESS_WIDTH-1 downto 0) := (others => '0'); + signal portn, portn_next : std_logic_vector(UDP_PORT_WIDTH-1 downto 0) := (others => '0'); + signal expects_inline_qos, expects_inline_qos_next : std_logic := '0'; + signal is_meta, is_meta_next : std_logic := '0'; + signal update_endpoint_flags : std_logic_vector(UPDATE_ENDPOINT_FLAG_WIDTH-1 downto 0) := (others => '0'); + signal opcode, opcode_next : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := (others => '0'); + signal flags, flags_next : std_logic_vector(SUBMESSAGE_FLAGS_WIDTH-1 downto 0) := (others => '0'); + signal seq_nr, seq_nr_next : SEQUENCENUMBER_TYPE := SEQUENCENUMBER_UNKNOWN; + signal next_seq_nr, next_seq_nr_next : SEQUENCENUMBER_TYPE := SEQUENCENUMBER_UNKNOWN; + signal ts, ts_next : TIME_TYPE := TIME_INVALID; + signal deadline, deadline_next : DURATION_TYPE := DURATION_ZERO; + signal sn_latch_1, sn_latch_1_next : SEQUENCENUMBER_TYPE := SEQUENCENUMBER_UNKNOWN; + signal sn_latch_2, sn_latch_2_next : SEQUENCENUMBER_TYPE := SEQUENCENUMBER_UNKNOWN; + signal sn_latch_3, sn_latch_3_next : SEQUENCENUMBER_TYPE := SEQUENCENUMBER_UNKNOWN; + signal bitmap_cnt, bitmap_cnt_next : unsigned(log2c(MAX_BITMAP_WIDTH/CDR_LONG_WIDTH)-1 downto 0) := (others => '0'); + signal bitmap_latch, bitmap_latch_next : BITMAP_TYPE := (others => (others => '0')); + signal cnt2, cnt2_next : natural range 0 to BITMAP_TYPE'length := 0; + signal bitmap_pos, bitmap_pos_next : natural range 0 to MAX_BITMAP_WIDTH-1 := 0; + signal key_hash, key_hash_next : KEY_HASH_TYPE := (others => (others => '0')); + signal key_hash_rcvd, key_hash_rcvd_next : std_logic := '0'; + signal status_info, status_info_next : std_logic_vector(STATUS_INFO_WIDTH-1 downto 0) := (others => '0'); + signal last_word_in_latch, last_word_in_latch_next : std_logic := '0'; + + -- Signal used to reset the word counter + signal reset_read_cnt : std_logic; + -- Word (4-Byte) counter (Counts words read from input fifo) + signal read_cnt : unsigned(SUBMESSAGE_LENGTH_WIDTH-3 downto 0) := (others => '0'); + -- Word aligned End of Parameter + signal parameter_end, parameter_end_next : unsigned(PARAMETER_LENGTH_WIDTH-1 downto 0) := (others => '0'); + + signal data_in_swapped : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0'); + + signal mem_stage, mem_stage_next : MEM_STAGE_TYPE := IDLE; + signal mem_wr, mem_rd : std_logic := '0'; + signal mem_wr_data, mem_rd_data : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0'); + signal mem_addr, mem_addr_next : unsigned(ENDPOINT_MEMORY_WIDTH-1 downto 0) := (others => '0'); + signal mem_addr_base, mem_addr_base_next : unsigned(ENDPOINT_MEMORY_WIDTH-1 downto 0) := (others => '0'); + signal addr_res, addr_res_next : unsigned(ENDPOINT_MEMORY_WIDTH-1 downto 0) := (others => '0'); + signal last_addr, last_addr_next : unsigned(ENDPOINT_MEMORY_WIDTH-1 downto 0) := (others => '0'); + signal max_endpoint_addr, max_endpoint_addr_next : unsigned(ENDPOINT_MEMORY_WIDTH-1 downto 0) := (others => '0'); + signal mem_cnt, mem_cnt_next : natural range TODO := 0; + signal mem_endpoint_data, mem_endpoint_data_next : ENDPOINT_DATA_TYPE := ZERO_ENDPOINT_DATA; + signal reset_max_pointer, reset_max_pointer_next : std_logic := '0'; + signal mem_long_latch, mem_long_latch_next : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0'); + signal mem_ctrl_data, mem_ctrl_data_next : MEM_CTRL_DATA_TYPE := ZERO_MEM_CTRL_DATA; + + signal stale_check, stale_check_next : std_logic := '0'; + signal count, count_next : unsigned(COUNT_WIDTH-1 downto 0) := (others => '0'); + signal return_stage, return_stage_next : STAGE_TYPE := IDLE; + + --*****ALIAS DECLARATION***** + -- ENDPOINT FRAME HEADER + 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); + -- RTPS PARAMETER LIST HEADER + 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 must_understand : std_logic is parameter_id(14); + -- RTPS DATA PAYLOAD HEADER + 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 qos_flag : std_logic is flags(1); + alias data_flag : std_logic is flags(2); + alias key_flag : std_logic is flags(3); + alias final_flag : std_logic is flags(1); + alias payload_flag : std_logic is flags(4); + -- HEARTBEAT + alias first_seq_nr : SEQUENCENUMBER_TYPE is sn_latch_1; + alias first_seq_nr_next : SEQUENCENUMBER_TYPE is sn_latch_1_next; + alias last_seq_nr : SEQUENCENUMBER_TYPE is sn_latch_2; + alias last_seq_nr_next : SEQUENCENUMBER_TYPE is sn_latch_2_next; + -- GAP + alias gap_start : SEQUENCENUMBER_TYPE is sn_latch_1; + alias gap_start_next : SEQUENCENUMBER_TYPE is sn_latch_1_next; + alias gap_list_base : SEQUENCENUMBER_TYPE is sn_latch_2; + alias gap_list_base_next : SEQUENCENUMBER_TYPE is sn_latch_2_next; + alias gap_list_end : SEQUENCENUMBER_TYPE is sn_latch_3; + alias gap_list_end_next : SEQUENCENUMBER_TYPE is sn_latch_3_next; + + -- FUNCTION DECLARATION + function to_slv_bitmap (input : BITMAP_TYPE) return std_logic_vector is + variable ret : std_logic_vector(0 to MAX_BITMAP_WIDTH-1) := (others => '0'); + begin + for i in 0 to BITMAP_TYPE'length-1 loop + ret(i*WORD_WIDTH to ((i+1)*WORD_WIDTH)-1) := input(i); + end loop; + return ret; + end function; + +begin + + --*****COMPONENT INSTANTIATION***** + ram_inst : single_port_ram + generic map ( + ADDR_WIDTH => ENDPOINT_MEMORY_WIDTH, + DATA_WIDTH => WORD_WIDTH, + MEMORY_DEPTH => ENDPOINT_MEMORY_SIZE + ) + port map ( + clk => clk, + addr => std_logic_vector(end_mem_addr), + wen => mem_wr, + ren => mem_rd, + wr_data => mem_wr_data, + rd_data => mem_rd_data + ); + + -- Big Endian Representation + data_in_swapped <= endian_swap(endian_flag, data_in); + + parse_prc : process(all) + variable tmp_dw : DOUBLE_WORD_ARRAY := (others => (others => '0')); + -- NOTE: We convert the bitamp to a slv to make operations easier (The tool should handle both cases equally) + variable tmp_bitmap : std_logic_vector(0 to MAX_BITMAP_WIDTH-1) := (others => '0'); + variable rd_guard : std_logic := '0'; + begin + -- DEFAULT + stage_next <= stage; + meta_opcode_next <= meta_opcode; + cnt_next <= cnt; + guid_next <= guid; + addr_next <= addr; + portn_next <= portn; + expects_inline_qos_next <= expects_inline_qos; + is_meta_next <= is_meta; + opcode_next <= opcode; + flags_next <= flags; + seq_nr_next <= seq_nr; + ts_next <= ts; + deadline_next <= deadline; + sn_latch_1_next <= sn_latch_1; + sn_latch_2_next <= sn_latch_2; + sn_latch_3_next <= sn_latch_3; + bitmap_pos_next <= bitmap_pos; + key_hash_rcvd_next <= key_hash_rcvd; + key_hash_next <= key_hash; + status_info_next <= status_info; + last_word_in_latch_next <= last_word_in_latch; + stale_check_next <= stale_check; + count_next <= count; + return_stage_next <= return_stage; + meta_rd <= '0'; + rd <= '0'; + mem_opcode <= NOP; + mem_op_start <= '0'; + rd_guard := '0'; + update_endpoint_flags <= (others => '0'); + hc_start <= '0'; + hc_opcode <= ADD_CACHE_CHANGE; + hc_data_in <= (others => '0'); + hc_ready_out <= '0'; + hc_valid_in <= '0'; + hc_last_word_in <= '0'; + + + -- Last Word Latch Setter + if (last_word_in = '1') then + last_word_in_latch_next <= '1'; + end if; + + case (meta_stage) is + -- OPCODE + when IDLE => + -- RESET + deadline_next <= TIME_INVALID; + addr_next <= (others => '0'); + portn_next <= (others => '0'); + status_info_next <= (others => '0'); + key_hash_rcvd_next <= '0'; + expects_inline_qos_next <= '0'; + is_meta_next <= '0'; + + -- Stale Entry search is initiated between every packet processing and when there is no packet to process + if (stale_check = '0' or (empty = '1' and meta_empty = '1')) then + if (mem_op_done = '1') then + mem_opcode <= FIND_STALE_ENDPOINT; + mem_op_start <= '1'; + + -- Stale Check Toggle (Setter) + stale_check_next <= '1'; + + stage_next <= ENDPOINT_STALE_CHECK; + end if; + -- Input FIFO Guard + elsif (meta_empty = '0') then + meta_rd <= '1'; + + -- Stale Check Toggle (Resetter) + stale_check_next <= '0'; + + -- Mark as METATRAFFIC + is_meta_next <= '1'; + + -- Latch Endpoint Metatraffic Opcode + meta_opcode_next <= meta_data_in; + + stage_next <= LATCH_GUIDPREFIX; + cnt_next <= 0; + -- Input FIFO Guard + elsif (empty = '0') then + rd_guard := '1'; + + -- Stale Check Toggle (Resetter) + stale_check_next <= '0'; + + -- Latch Opcode + opcode_next <= header_opcode; + -- Latch Flags + flags_next <= header_flags; + -- Latch Source UDP Port + portn_next <= header_udp_port; + + stage_next <= LATCH_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 LATCH_GUIDPREFIX => + -- Input FIFO Guard + if ((is_meta = '1' and meta_empty = '0') or (is_meta = '0' and empty = '0')) then + if (is_meta = '1') then + meta_rd <= '1'; + else + rd_guard := '1'; + end if; + cnt_next <= cnt + 1; + + case (cnt) is + -- GUID Prefix 1/3 + when 0 => + if (is_meta = '1') then + guid_next(0) <= meta_data_in; + else + guid_next(0) <= data_in; + end if; + -- GUID Prefix 2/3 + when 1 => + if (is_meta = '1') then + guid_next(1) <= meta_data_in; + else + guid_next(1) <= data_in; + end if; + -- GUID Prefix 3/3 + when 2 => + if (is_meta = '1') then + guid_next(2) <= meta_data_in; + else + guid_next(2) <= data_in; + end if; + + if (is_meta = '1' and (meta_opcode = OPCODE_PARTICIPANT_UNMATCH or meta_opcode = OPCODE_LIVELINESS_UPDATE)) then + -- DONE Parsing + stage_next <= METATRAFFIC_OPERATION; + else + stage_next <= LATCH_ENTITYID; + end if; + end case; + end if; + when LATCH_ENTITYID => + -- Input FIFO Guard + if ((is_meta = '1' and meta_empty = '0') or (is_meta = '0' and empty = '0')) then + if (is_meta = '1') then + meta_rd <= '1'; + guid_next(3) <= meta_data_in; + + if (mem_opcode = OPCODE_ENDPOINT_MATCH) then + stage_next <= LATCH_ENDPOINT_DATA; + cnt_next <= 0; + else + stage_next <= METATRAFFIC_OPERATION; + end if; + -- Memory Operation Guard + else + rd_guard := '1'; + guid_next(3) <= data_in; + + stage_next <= INITIATE_ENDPOINT_SEARCH; + end if; + end if; + when LATCH_ENDPOINT_DATA => + -- Input FIFO Guard + if (meta_empty = '0') then + meta_rd <= '1'; + cnt_next <= cnt + 1; + + case (cnt) is + -- IPv4 Address + when 0 => + addr_next <= meta_data_in; + -- UDP Port & EXTRA Flags (Expects InLine QoS) + when 1 => + portn_next <= meta_data_in(WORD_WIDTH-1 downto WORD_WIDTH-UDP_PORT_WIDTH-1); + expects_inline_qos_next <= meta_data_in(0); + -- Calculate Lease Deadline + deadline_next <= time + ENDPOINT_LEASE_DURATION(ID); + + stage_next <= METATRAFFIC_OPERATION; + when others => + null; + end case; + end if; + when METATRAFFIC_OPERATION => + -- Memory Operation Guard + if (mem_op_done = '1') then + + case (meta_opcode) is + when OPCODE_ENDPOINT_MATCH => + -- Insert Matched Remote Endpoint + -- NOTE: The Lease Duration is NOT updated in case of an update. That is the responsibility of the Liveliness Update + update_endpoint_flags <= ENDPOINT_DATA_FLAG; -- In case the Endpoint is already in the memory, we update the data + mem_opcode <= INSERT_ENDPOINT; + mem_op_start <= '1'; + stage_next <= IDLE; + when OPCODE_ENDPOINT_UNMATCH => + -- Remove Unmatched Remote Endpoint + mem_opcode <= REMOVE_ENDPOINT; + mem_op_start <= '1'; + stage_next <= IDLE; + when OPCODE_PARTICIPANT_UNMATCH => + -- Remove All Endpoint of Remote Participant + mem_opcode <= REMOVE_PARTICIPANT; + mem_op_start <= '1'; + stage_next <= IDLE; + when OPCODE_LIVELINESS_UPDATE => + -- Renew Lease of Remote Endpoint + update_endpoint_flags <= LEASE_DEADLINE_FLAG; + mem_opcode <= UDPATE_ENDPOINT; + mem_op_start <= '1'; + stage_next <= IDLE; + when others => + assert FALSE report "Uknown metatraffic endpoint frame opcode." severity FAILURE; + null; + end case; + -- DONE + stage_next <= SKIP_PACKET; + end if; + when LATCH_SRC_ADDR => + -- Input FIFO Guard + if (empty = '0') then + rd_guard := '1'; + + -- Latch Source IP Address + addr_next <= data_in; + + stage_next <= LATCH_GUIDPREFIX; + cnt_next <= 0; + end if; + when INITIATE_ENDPOINT_SEARCH => + -- Memory Operation Guard + if (mem_op_done = '1') then + mem_op_start <= '1'; + mem_opcode <= SEARCH_ENDPOINT; + + case (opcode) is + when SID_DATA => + stage_next <= LATCH_EXTRA_DATA; + cnt_next <= 0; + when SID_HEARTBEAT => + stage_next <= LATCH_HEARTBEAT; + cnt_next <= 0; + when SID_GAP => + stage_next <= LATCH_GAP; + cnt_next <= 0; + when others => + stage_next <= SKIP_PACKET; + end case; + end if; + when LATCH_EXTRA_DATA => + -- Input FIFO Guard + if (empty = '0') then + rd_guard := '1'; + cnt_next <= cnt + 1; + + case (cnt) is + -- Sequence Number 1/2 + when 0 => + seq_nr_next(0) <= unsigned(data_in); + -- Sequence Number 2/2 + when 1 => + seq_nr_next(1) <= unsigned(data_in); + -- Store Next Sequence Number + tmp_dw := (0 => seq_nr(0), 1 => unsigned(data_in_swapped)); + next_seq_nr_next <= tmp_dw + 1; + -- Timestamp 1/2 + when 2 => + ts_next(0) <= unsigned(data_in); + -- Timestamp 2/2 + when 3 => + ts_next(1) <= unsigned(data_in); + + if (qos_flag = '1') then + stage_next <= PROCESS_INLINE_QOS; + else + stage_next <= INITIATE_HISTORY_CACHE_REQUEST; + end if; + when others => + null; + end case; + end if; + when LATCH_HEARTBEAT => + -- Input FIFO Guard + if (empty = '0') then + rd_guard := '1'; + cnt_next <= cnt + 1; + + case (cnt) is + -- First Sequence Number 1/2 + when 0 => + first_seq_nr_next(0) <= unsigned(data_in); + -- First Sequence Number 2/2 + when 1 => + first_seq_nr_next(1) <= unsigned(data_in); + -- Last Sequence Number 1/2 + when 2 => + last_seq_nr_next(0) <= unsigned(data_in); + -- Last Sequence Number 2/2 + when 3 => + last_seq_nr_next(1) <= unsigned(data_in); + + stage_next <= PROCESS_HEARTBEAT; + when others => + null; + end case; + end if; + when PROCESS_HEARTBEAT => + -- Wait for Endpoint Search to finish + if (mem_op_done = '1') then + -- DEFAULT + stage_next <= SKIP_PACKET; + + -- Endpoint in Buffer + if (addr_res /= MAX_ADDRESS) then + -- No scheduled Heartbeat Response + if (mem_endpoint_data.res_time = 0) then + -- If current Sequence Number obsolete (removed from source history cache) + if (first_seq_nr > mem_endpoint_data.next_seq_nr and first_seq_nr <= last_seq_nr) then + -- Store new expected Sequence Number and set Response Dealy + next_seq_nr_next <= first_seq_nr; + mem_op_start <= '1'; + mem_opcode <= UPDATE_ENDPOINT; + -- XXX: Assumes lease deadline will not be updated concurrently (Since they use the same latch) + deadline_next <= time + ENDPOINT_HEARTBEAT_RESPONSE_DELAY(ID); + -- NOTE: Last Bit denotes if this is Response or Suppression Delay + deadline_next(1)(0) <= '0'; + -- Update also highest available sequence number if necessary + if (last_seq_nr /= mem_endpoint_data.highest_seq_nr) then + update_endpoint_flags <= NEXT_SEQ_NR_FLAG or HIGHEST_SEQ_NR_FLAG or RES_TIME_FLAG; + else + update_endpoint_flags <= NEXT_SEQ_NR_FLAG or RES_TIME_FLAG; + end if; + -- If new Sequence Number is available or Writer expects ACKNACK + elsif (last_seq_nr >= mem_endpoint_data.next_seq_nr or final_flag = '0') then + -- Set Response Delay + mem_opcode <= UPDATE_ENDPOINT; + mem_op_start <= '1'; + deadline_next <= time + ENDPOINT_HEARTBEAT_RESPONSE_DELAY(ID); + -- NOTE: Last Bit denotes if this is Response or Suppression Delay + deadline_next(1)(0) <= '0'; + -- Update also highest available sequence number if necessary + if (last_seq_nr /= mem_endpoint_data.highest_seq_nr) then + update_endpoint_flags <= HIGHEST_SEQ_NR_FLAG or RES_TIME_FLAG; + else + update_endpoint_flags <= RES_TIME_FLAG; + end if; + end if; + -- Currently in Heartbeat Response Delay + elsif (mem_participant_data.res_time(1)(0) = '0') then + -- If current Sequence Number obsolete (removed from source history cache) + if (first_seq_nr > mem_endpoint_data.next_seq_nr and first_seq_nr <= last_seq_nr) then + -- Store new expected Sequence Number + next_seq_nr_next <= first_seq_nr; + mem_op_start <= '1'; + mem_opcode <= UPDATE_ENDPOINT; + -- Update also highest available sequence number if different + if (last_seq_nr /= mem_endpoint_data.highest_seq_nr) then + update_endpoint_flags <= NEXT_SEQ_NR_FLAG or HIGHEST_SEQ_NR_FLAG; + else + update_endpoint_flags <= NEXT_SEQ_NR_FLAG; + end if; + elsif (last_seq_nr /= mem_endpoint_data.highest_seq_nr and first_seq_nr <= last_seq_nr) then + -- Store new highest available sequence number + mem_op_start <= '1'; + mem_opcode <= UPDATE_ENDPOINT; + update_endpoint_flags <= HIGHEST_SEQ_NR_FLAG; + end if; + end if; + end if; + end if; + when LATCH_GAP => + -- Input FIFO Guard + if (empty = '0') then + rd_guard := '1'; + cnt_next <= cnt + 1; + + case (cnt) is + -- GapStart Sequence Number 1/2 + when 0 => + gap_start_next(0) <= unsigned(data_in); + -- GapStart Sequence Number 2/2 + when 1 => + gap_start_next(1) <= unsigned(data_in); + -- GapList.Base 1/2 + when 2 => + gap_list_base_next(0) <= unsigned(data_in); + -- GapList.Base 2/2 + when 3 => + gap_list_base_next(1) <= unsigned(data_in); + -- ReaderSNState.NumBits + when 4 => + gap_list_end_next <= gap_list_base + to_integer(unsigned(data_in)); + bitmap_cnt_next <= unsigned(round_slv(data_in(log2c(MAX_BITMAP_WIDTH)-1 downto 0),bitmap_cnt'length)); + cnt2_next <= 0; + -- ReaderSNState.Bitmap + when 5 => + -- Read Bitmap + if (cnt2 < bitmap_cnt) then + cnt2_next <= cnt2 + 1; + + bitmap_latch_next(cnt2) <= data_in_swapped; + + -- Keep Sub-State + cnt_next <= cnt; + else + stage_next <= PROCESS_GAP; + end if; + when others => + null; + end case; + + end if; + when PROCESS_GAP => + -- Wait for Endpoint Search + if (mem_op_done = '1') then + -- DEFAULT + stage_next <= SKIP_PACKET; + + -- Known Remote Endpoint + if (addr_res /= MAX_ADDRESS) then + -- GAP is relevant + if (gap_start <= mem_endpoint_data.next_seq_nr and mem_endpoint_data.next_seq_nr <= gap_list_end) then + -- Next Expected is in GAP List + if (gap_list_base <= mem_endpoint_data.next_seq_nr) then + -- Begin searching next valid SN from the current expected in the bitmap list + bitmap_pos_next <= to_integer(next_seq_nr - gap_list_base); + else + -- Begin searching next valid SN from the beginning of the bitmap list + next_seq_nr_next <= gap_list_base; + bitmap_pos_next <= 0; + end if; + stage_next <= FIND_NEXT_VALID_IN_BITMAP; + end if; + end if; + end if; + when FIND_NEXT_VALID_IN_BITMAP => + -- Memory Operation Guard + if (mem_op_done = '1') then + tmp_bitmap := to_slv_bitmap(bitmap_latch); + + -- TODO: Test GAP that has next expected not marked in GAP + + -- First valid sequence number found + if (tmp_bitmap(bitmap_pos) = '0') then + -- Update next sequence number + mem_op_start <= '1'; + mem_opcode <= UPDATE_ENDPOINT; + update_endpoint_flags <= NEXT_SEQ_NR_FLAG; + -- DONE + stage_next <= SKIP_PACKET; + else + -- Continue search + bitmap_pos_next <= bitmap_pos + 1; + next_seq_nr_next <= next_seq_nr + 1; + end if; + end if; + when PROCESS_INLINE_QOS => + -- input FIFO Guard + if (empty = '0') then + rd_guard := '1'; + -- Reset Word Counter + reset_read_cnt <= '1'; + -- Latch Parameter End (In order to skip parameters) + parameter_end_next <= unsigned(endian_swap(endian_flag,parameter_length)); + -- DEFAULT STAGE + stage_next <= SKIP_PARAMETER; + + case (parameter_id) is + when PID_TOPIC_NAME => + -- Ignore + null; + when PID_DURABILITY => + -- Ignore + null; + when PID_PRESENTATION => + -- Ignore + null; + when PID_DEADLINE => + -- Ignore + null; + when PID_LATENCY_BUDGET => + -- Ignore + null; + when PID_OWNERSHIP => + -- Ignore + null; + when PID_OWNERSHIP_STRENGTH => + -- Ignore + null; + when PID_LIVELINESS => + -- Ignore + null; + when PID_PARTITION => + -- Ignore + null; + when PID_RELIABILITY => + -- Ignore + null; + when PID_DESTINATION_ORDER => + -- Ignore + null; + when PID_TRANSPORT_PRIORITY => + -- Ignore + null; + when PID_LIFESPAN => + stage_next <= LATCH_LIFESPAN; + cnt_next <= 0; + when PID_CONTENT_FILTER_INFO => + -- Ignore + null; + when PID_COHERENT_SET => + -- Ignore + null; + when PID_DIRECTED_WRITE => + -- Ignore + null; + when PID_ORIGINAL_WRITER_INFO => + -- Ignore + null; + when PID_GROUP_COHERENT_SET => + -- Ignore + null; + when PID_GROUP_SEQ_NUM => + -- Ignore + null; + when PID_WRITER_GROUP_INFO => + -- Ignore + null; + when PID_SECURE_WRITER_GROUP_INFO => + -- Ignore + null; + when PID_KEY_HASH => + key_hash_rcvd_next <= '1'; + stage_next <= LATCH_KEY_HASH; + cnt_next <= 0; + when PID_STATUS_INFO => + stage_next <= LATCH_STATUS_INFO; + when PID_PAD => + -- Ignore + null; + when PID_SENTINEL => + -- QOS DONE + stage_next <= INITIATE_HISTORY_CACHE_REQUEST; + when others => + -- If MUST_UNDERSTAND Flag is set, we have incompatible communication. Drop Packet + if (must_understand = '1') then + stage_next <= SKIP_PACKET; + -- Else skip Unknown Parameter + else + stage_next <= SKIP_PARAMETER; + end if; + end case; + end if; + when LATCH_LIFESPAN => + -- Input FIFO Guard + if (empty = '0') then + rd_guard := '1'; + cnt_next <= cnt + 1; + + case (cnt) is + -- Lifespan 1/2 + when 0 => + deadline_next(0) <= unsigned(data_in_swapped); + -- Lifespan 2/2 + when 1 => + deadline_next(1) <= unsigned(data_in_swapped); + + -- DONE + stage_next <= SKIP_PARAMETER; + when others => + null; + end case; + end if; + when LATCH_KEY_HASH => + -- Input FIFO Guard + if (empty = '0') then + rd_guard := '1'; + cnt_next <= cnt + 1; + + case (cnt) is + -- Key Hash 1/4 + when 0 => + key_hash_next(0) <= data_in; + -- Key Hash 2/4 + when 1 => + key_hash_next(1) <= data_in; + -- Key Hash 3/4 + when 2 => + key_hash_next(2) <= data_in; + -- Key Hash 4/4 + when 3 => + key_hash_next(3) <= data_in; + + -- DONE + stage_next <= SKIP_PARAMETER; + when others => + null; + end case; + end if; + when LATCH_STATUS_INFO => + -- Input FIFO Guard + if (empty = '0') then + rd_guard := '1'; + + status_info_next <= data_in; + + -- DONE + stage_next <= SKIP_PARAMETER; + end if; + when INITIATE_HISTORY_CACHE_REQUEST => + hc_start <= '1'; + hc_opcode <= ADD_CACHE_CHANGE; + -- Wait until History Cache acknowledges request + if (hc_ack = '1') then + stage_next <= ADD_CACHE_CHANGE; + cnt_next <= 0; + end if; + when ADD_CACHE_CHANGE => + if (hc_ready_in = '1') then + hc_valid_in <= '1'; + cnt_next <= cnt + 1; + + case (cnt) is + -- Status Info + when 0 => + hc_data_in <= status_info; + hc_data_in(KEY_HASH_FLAG) <= key_hash_rcvd; + hc_data_in(PAYLOAD_FLAG) <= data_flag; + -- Key hash 1/4 + when 1 => + hc_data_in <= key_hash(0); + -- Key Hash 2/4 + when 2 => + hc_data_in <= key_hash(1); + -- Key Hash 3/4 + when 3 => + hc_data_in <= key_hash(2); + -- Key hash 4/4 + when 4 => + hc_data_in <= key_hash(3); + -- Entity ID + when 5 => + hc_data_in <= guid(3); + -- GUID Prefix 1/3 + when 6 => + hc_data_in <= guid(0); + -- GUID Prefix 2/3 + when 7 => + hc_data_in <= guid(1); + -- GUID Prefix 3/3 + when 8 => + hc_data_in <= guid(2); + -- Sequence Number 1/2 + when 9 => + hc_data_in <= seq_nr(0); + -- Sequence Number 2/2 + when 10 => + hc_data_in <= seq_nr(1); + -- Timestamp 1/2 + when 11 => + hc_data_in <= ts(0); + -- Timestamp 2/2 + when 12 => + hc_data_in <= ts(1); + -- Lifespan Deadline 1/2 + when 13 => + hc_data_in <= deadline(0); + -- Lifespan Deadline 2/2 + when 14 => + hc_data_in <= deadline(1); + + -- Payload exists + if (data_flag = '1' or key_flag = '1') then + stage_next <= PUSH_PAYLOAD; + else + -- DONE + hc_last_word_in <= '1'; + + -- Operation Sucessfull + if (hc_ack = '1') then + stage_next <= FINALIZE_HISTORY_CACHE_REQUEST; + else + -- Operation Failed, Skip + stage_next <= SKIP_PACKET; + end if; + end if; + when others => + null; + end case; + end if; + when PUSH_PAYLOAD => + -- Input/Output Guard + if (empty = '0' and hc_ready_in = '1') then + rd_guard := '1'; + hc_valid_in <= '1'; + + -- Push Payload to History Cache + hc_data_in <= data_in; + + -- Exit Condition + if (last_word_in = '1') then + hc_last_word_in <= '1'; + -- XXX: Possible worst case path (The hc_ack is set from last_word signal -> round trip delay between entities) + -- Change Add Sucessfull + if (hc_ack = '1') then + -- Update Endpoint Data + stage_next <= FINALIZE_HISTORY_CACHE_REQUEST; + else + -- Operation Failed, Skip + stage_next <= SKIP_PACKET; + end if; + end if; + end if; + when FINALIZE_HISTORY_CACHE_REQUEST => + -- Memory Operation Guard + if (mem_op_done = '1') then + -- Update next sequence number and renew Lease + mem_op_start <= '1'; + mem_opcode <= UPDATE_ENDPOINT; + deadline_next <= time + ENDPOINT_LEASE_DURATION(ID); + update_endpoint_flags <= NEXT_SEQ_NR_FLAG or LEASE_DEADLINE_FLAG; + -- DONE + stage_next <= SKIP_PACKET; + end if; + when ENDPOINT_STALE_CHECK => + -- Wait for Stale Search to finish + if (mem_op_done = '1') then + -- Found Stale Entry + if (addr_res /= MAX_ADDRESS) then + -- Endpoint Lease Expired + -- NOTE: The mem_endpoint_data is zero initialized on lease expiration, so we check if the address is set + if (mem_endpoint_data.addr = IPv4_ADDRESS_INVALID) then + -- Remove Participant + mem_opcode <= REMOVE_ENDPOINT; + mem_op_start <= '1'; + stage_next <= IDLE; + -- Response Time Reached + else + -- If Suppression Delay passed, zero the time + if(mem_endpoint_data.res_time(1)(0) = '1') then + -- Zero Response Time + mem_opcode <= UPDATE_ENDPOINT; + deadline_next <= TIME_ZERO; + update_participant_flags <= RES_TIME_FLAG; + mem_op_start <= '1'; + -- DONE + stage_next <= IDLE; + -- If Response Delay Passed + else + -- Set Heartbeat Suppression Time + if (ENDPOINT_HEARTBEAT_SUPPRESSION_DELAY(ID) /= 0) then + -- Set Heartbeat Suppression Time + deadline_next <= time + ENDPOINT_HEARTBEAT_SUPPRESSION_DELAY(ID); + -- NOTE: Last Bit denotes if this is Response or Suppression Delay + deadline_next(1)(0) <= '1'; + else + -- Zero Heartbeat Response Time + deadline_next <= TIME_ZERO; + end if; + mem_opcode <= UPDATE_PARTICIPANT; + update_participant_flags_next <= RES_TIME_FLAG; + mem_op_start <= '1'; + -- Send ACKNACK + -- Increment Heartbeat/Acknack Counter + count_next <= count + 1; + stage_next <= SEND_HEADER; + return_stage_next <= SEND_ACKNACK; + cnt_next <= 0; + end if; + end if; + -- No Stale Entry Found + else + -- DONE + stage_next <= IDLE; + end if; + end if; + when SEND_HEADER => + if (rtps_full = '0') then + wr_sig <= '1'; + cnt_next <= cnt + 1; + + case (cnt) is + -- OUTPUT HEADER + -- Src IPv4 Address + when 0 => + data_out <= DEFAULT_IPv4_ADDRESS; + -- Dest IPv4 Address + when 1 => + data_out <= mem_endpoint_data.addr; + -- Src and Dest UDPv4 Ports + when 2 => + data_out <= USER_IPv4_UNICAST_PORT & mem_participant_data.meta_port; + -- RTPS MESSAGE HEADER + when 3 => + data_out <= PROTOCOL_RTPS; + when 4 => + data_out <= PROTOCOLVERSION_2_4 & VENDORID; + when 5 => + data_out <= GUIDPREFIX(0); + when 6 => + data_out <= GUIDPREFIX(1); + when 7 => + data_out <= GUIDPREFIX(2); + -- Continue with respective RTPS Submessage + stage_next <= return_stage; + cnt_next <= 0; + when others => + null; + end case; + end if; + when SEND_ACKNACK => + if (rtps_full = '0') then + wr_sig <= '1'; + cnt_next <= cnt + 1; + + case (cnt) is + -- ACKNACK RTPS SUBMESSAGE + -- RTPS Submessage Header + when 0 => + data_out <= SID_ACKNACK & "00000010" & std_logic_vector(to_unsigned(28, SUBMESSAGE_LENGTH_WIDTH)); + -- Reader Entity ID + when 1 => + data_out <= ENTITYID(ID); + -- Writer Entity ID + when 2 => + data_out <= ENTITYID_UNKNOWN; + -- Sequence Number Set (Bitmap Base 1/2) + when 3 => + data_out <= std_logic_vector(mem_endpoint_data.next_seq_nr(0)); + -- Sequence Number Set (Bitmap Base 2/2) + when 4 => + data_out <= std_logic_vector(mem_endpoint_data.next_seq_nr(1)); + -- Sequence Number Set (NumBits) + when 5 => + data_out <= std_logic_vector(to_unsigned(CDR_LONG_WIDTH, CDR_LONG_WIDTH)); + -- Sequence Number Set (Bitmap) + when 6 => + -- NOTE: In order to avoid having to generate a variable sized bitmap, we always request the next 32 sequence numbers, even if they do not exist (yet) + -- XXX: Assumes correct implementation of the RTPS Protocol (i.e. Writer ignores requested SNs that do not exist) + data_out <= (others => '1'); + -- Count + when 7 => + data_out <= std_logic_vector(count); + last_word_out <= '1'; + + -- DONE + stage_next <= IDLE; + when others => + null; + end case; + end if; + when SKIP_PARAMETER => + -- Consumed last word of Packet + if (last_word_in_latch = '1' and last_word_in = '0') then + -- Reset Last Word In Latch + last_word_in_latch_next <= '0'; + -- Continue parsing next Packet + stage_next <= IDLE; + -- Reset Parameter End + parameter_end_next <= (others => '1'); + -- End of Parameter + elsif ((read_cnt & "00" ) >= parameter_end) then + -- Parse Next Parameter + -- NOTE: data_in is already showing the next parameter + stage_next <= PROCESS_PL; + -- Reset Parameter End + parameter_end_next <= (others => '1'); + -- Input FIFO Guard + elsif (empty = '0') then + -- Skip-Read + rd_guard := '1'; + end if; + when SKIP_PACKET => + -- NOTE: At the end of a Stale Entry Removal this stage is entered, without having started reading a Packet from input. + -- Reset Parameter End + parameter_end_next <= (others => '1'); + + -- Stale Check Exit + if (stale_check = '1') then + -- DONE + stage_next <= IDLE; + -- Consumed last word of Packet + elsif (last_word_in_latch = '1' and last_word_in = '0') then + -- Reset Last Word In Latch + last_word_in_latch_next <= '0'; + -- DONE + stage_next <= IDLE; + -- Input FIFO Guard + elsif (empty = '0') then + -- Skip-Read + rd_guard := '1'; + end if; + when others => + null; + end case; + + -- OVERREAD GUARD + -- Read outside of packet Length + -- NOTE: If the Packet Length is smaller than expected there will be a read from input FIFO while + -- the Packet Length has been reached and will be caught by this clause. + -- The SKIP_PACKET clause prevents a read signal from occuring in this situation, and thus prevents from entering this state. + if ((last_word_in_latch = '1' and last_word_in = '0') and rd_guard = '1') then + -- Force rd_sig low + rd_sig <= '0'; + -- Continue parsing next Packet + stage_next <= IDLE; + -- Reset Last Word In Latch + last_word_in_latch_next <= '0'; + -- Reset Parameter End + parameter_end_next <= (others => '1'); + -- Read outside of Parameter Length + -- NOTE: If the Parameter Length is smaller than expected for a particular parameter, there will be a read from input FIFO while + -- the Parameter Length has been reached and will be caught by this clause. + -- The SKIP_PARAMETER clause prevents a read signal from occuring in this situation, and thus prevents from entering this state. + elsif ((read_cnt & "00") >= parameter_end and rd_guard = '1') then + -- Force rd_sig low + rd_sig <= '0'; + -- Invalid Parameter Length, Skip Packet + stage_next <= SKIP_PACKET; + -- Reset Parameter End + parameter_end_next <= (others => '1'); + -- DEFAULT + else + rd_sig <= rd_guard; + end if; + end process; + + mem_ctrl_prc : process(all) + variable tmp : unsigned(mem_addr_base'range) := (others => '0'); + variable tmp2 : unsigned(mem_addr_base'range) := (others => '0'); + variable tmp3 : unsigned(mem_addr_base'range) := (others => '0'); + variable tmp_dw : DOUBLE_WORD_ARRAY := (others => (others => '0')); + begin + -- DEFAULT Registered + mem_stage_next <= mem_stage; + mem_addr_base_next <= mem_addr_base; + mem_addr_next <= mem_addr; + addr_res_next <= addr_res; + mem_cnt_next <= mem_cnt; + last_addr_next <= last_addr; + mem_endpoint_data_next <= mem_endpoint_data; + mem_guidprefix_next <= mem_guidprefix; + max_endpoint_addr_next <= max_endpoint_addr; + reset_max_pointer_next <= reset_max_pointer; + mem_long_latch_next <= mem_long_latch; + mem_ctrl_data_next <= mem_ctrl_data; + -- DEFAULT Unregistered + mem_write_data <= (others => '0'); + mem_op_done <= '0'; + mem_rd <= '0'; + mem_wr <= '0'; + + + case (mem_stage) is + when IDLE => + mem_op_done <= '1'; + reset_max_pointer_next <= '0'; + + if (mem_op_start = '1') then + -- Latch Signals needed for Mermory Operation (Use _next signals, because some signals are set in same clk) + mem_ctrl_data_next <= ( + guid => guid_next, + addr => addr_next, + portn => portn_next, + expects_inline_qos => expects_inline_qos_next, + deadline => deadline_next, + next_seq_nr => next_seq_nr_next, + highest_seq_nr => last_seq_nr_next, + update_flags => update_endpoint_flags_next, + mem_opcode => mem_opcode_next + ); + + case(mem_opcode) is + when SEARCH_ENDPOINT => + mem_addr_base_next <= FIRST_ENDPOINT_ADDRESS; + mem_addr_next <= FIRST_ENDPOINT_ADDRESS; + mem_stage_next <= SEARCH_ENDPOINT; + mem_cnt_next <= 0; + when INSERT_ENDPOINT => + -- NOTE: First check if Endpoint is already inserted. If yes, just update the data. + mem_addr_base_next <= FIRST_ENDPOINT_ADDRESS; + mem_addr_next <= FIRST_ENDPOINT_ADDRESS; + mem_stage_next <= SEARCH_ENDPOINT; + mem_cnt_next <= 0; + when UPDATE_ENDPOINT => + if ((update_endpoint_flags and ENDPOINT_DATA_FLAG) = ENDPOINT_DATA_FLAG) then + mem_stage_next <= UPDATE_ENDPOINT; + mem_addr_next <= addr_res + 4; + mem_cnt_next <= 0; + elsif ((update_endpoint_flags and LEASE_DEADLINE_FLAG) = LEASE_DEADLINE_FLAG) then + mem_stage_next <= UPDATE_ENDPPOINT; + mem_addr_next <= addr_res + 6; + mem_cnt_next <= 2; + elsif ((update_endpoint_flags and RES_TIME_FLAG) = RES_TIME_FLAG) then + mem_stage_next <= UPDATE_ENDPOINT; + mem_addr_next <= addr_res + 8; + mem_cnt_next <= 4; + elsif ((update_endpoint_flags and NEXT_SEQ_NR_FLAG) = NEXT_SEQ_NR_FLAG) then + mem_stage_next <= UPDATE_ENDPOINT; + mem_addr_next <= addr_res + 10; + mem_cnt_next <= 6; + elsif ((update_endpoint_flags and HIGHEST_SEQ_NR_FLAG) = HIGHEST_SEQ_NR_FLAG) then + mem_stage_next <= UPDATE_ENDPOINT; + mem_addr_next <= addr_res + 12; + mem_cnt_next <= 8; + end if; + when REMOVE_ENDPOINT => + mem_addr_next <= addr_res; + mem_stage_next <= REMOVE_ENDPOINT; + mem_cnt_next <= 0; + when REMOVE_PARTICIPANT => + mem_addr_base_next <= FIRST_ENDPOINT_ADDRESS; + mem_addr_next <= FIRST_ENDPOINT_ADDRESS; + mem_stage_next <= SEARCH_ENDPOINT; + mem_cnt_next <= 0; + when FIND_STALE_ENDPOINT => + mem_addr_base_next <= FIRST_ENDPOINT_ADDRESS; + mem_addr_next <= FIRST_ENDPOINT_ADDRESS; + mem_stage_next <= FIND_STALE_ENDPOINT; + mem_cnt_next <= 0; + when FIND_FIRST_ENDPOINT => + mem_addr_base_next <= FIRST_ENDPOINT_ADDRESS; + mem_addr_next <= FIRST_ENDPOINT_ADDRESS; + mem_stage_next <= FIND_NEXT_ENDPOINT; + mem_cnt_next <= 0; + when FIND_NEXT_ENDPOINT => + -- Memory Bound Guard + if (addr_res /= max_endpoint_addr) then + -- Reached End of Memory, No match + tmp := addr_res + ENDPOINT_FRAME_SIZE; + mem_addr_base_next <= tmp; + mem_addr_next <= tmp; + mem_stage_next <= FIND_NEXT_ENDPOINT; + mem_cnt_next <= 0; + else + addr_res_next <= MAX_ADDRESS; + end if; + when others => + null; + end case; + end if; + when SEARCH_ENDPOINT => + mem_rd <= '1'; + mem_cnt_next <= mem_cnt + 1; + mem_addr_next <= mem_addr + 1; + + -- Next Endpoint Frame Address + tmp := mem_addr_base + ENDPOINT_FRAME_SIZE; + + case (mem_cnt) is + -- Preload + when 0 => + null; + -- Entity ID + when 1 => + -- No Match (Ignore Entity ID match on Participant Search, but skip empty Slot) + if ((mem_read_data /= guid(3) and mem_ctrl_data.mem_opcode /= REMOVE_PARTICIPANT) or (mem_read_data = ENTITYID_UNKNOWN)) then + -- Reached End of Memory, No Match + if (mem_addr_base = max_endpoint_addr) then + case (mem_ctrl_data.mem_opcode) is + when INSERT_ENDPOINT => + -- Insert new Endpoint + mem_addr_base_next <= FIRST_ENDPOINT_ADDRESS; + mem_addr_next <= FIRST_ENDPOINT_ADDRESS; + mem_stage_next <= FIND_ENDPOINT_SLOT; + mem_cnt_next <= 0; + when REMOVE_PARTICIPANT => + -- Reset MAX Endpoint Pointer + mem_addr_base_next <= FIRST_ENDPOINT_ADDRESS; + mem_addr_next <= FIRST_ENDPOINT_ADDRESS; + reset_max_pointer_next <= '1'; + last_addr_next <= (others => '0'); + mem_stage_next <= FIND_ENDPOINT_SLOT; + mem_cnt_next <= 0; + when others => + addr_res_next <= MAX_ADDRESS; --No match + -- DONE + mem_stage_next <= IDLE; + end case; + else + -- Continue Search + mem_addr_next <= tmp; + mem_addr_base_next <= tmp; + mem_cnt_next <= 0; + end if; + end if; + -- GUID Prefix 1/3 + when 2 => + -- No Match + if (mem_read_data /= guid(0)) then + -- Continue Search + mem_addr_next <= tmp; + mem_addr_base_next <= tmp; + mem_cnt_next <= 0; + end if; + -- GUID Prefix 2/3 + when 3 => + -- No Match + if (mem_read_data /= guid(1)) then + -- Continue Search + mem_addr_next <= tmp; + mem_addr_base_next <= tmp; + mem_cnt_next <= 0; + end if; + -- GUID Prefix 3/3 + when 4 => + -- No Match + if (mem_read_data /= guid(2)) then + -- Continue Search + mem_addr_next <= tmp; + mem_addr_base_next <= tmp; + mem_cnt_next <= 0; + -- Match + else + case (mem_ctrl_data.mem_opcode) is + when REMOVE_PARTICIPANT => + addr_res_next <= mem_addr_base; + -- Remove Endpoint from remote Participant + mem_stage_next <= REMOVE_ENDPOINT; + mem_addr_next <= mem_addr_base; + mem_cnt_next <= 0; + when INSERT_ENDPOINT => + addr_res_next <= mem_addr_base; + -- Update Endpoint Data + mem_stage_next <= UPDATE_ENDPOINT; + mem_addr_next <= mem_addr_base + 4; + mem_cnt_next <= 0; + when others => + addr_res_next <= mem_addr_base; + -- Fetch Endpoint Data + mem_stage_next <= GET_ENDPOINT_DATA; + mem_cnt_next <= 1; -- No preload needed + end case; + end if; + when others => + null; + end case; + when GET_ENDPOINT_DATA => + mem_rd <= '1'; + mem_cnt_next <= mem_cnt + 1; + mem_addr_next <= mem_addr + 1; + + case (mem_cnt) is + -- Memory Preload + when 0 => + null; + -- IPv4 Address + when 1 => + mem_endpoint_data_next.addr <= mem_read_data; + -- UDPv4 Ports + when 2 => + mem_endpoint_data_next.portn <= mem_read_data(31 downto 16); + mem_endpoint_data_next.expects_inline_qos <= mem_read_data(0); + -- Lease Deadline 1/2 + when 3 => + mem_endpoint_data_next.lease_deadline(0) <= unsigned(mem_read_data); + -- Lease Deadline 2/2 + when 4 => + mem_endpoint_data_next.lease_deadline(1) <= unsigned(mem_read_data); + -- Response/Suppression Time 1/2 + when 5 => + mem_endpoint_data_next.res_time(0) <= unsigned(mem_read_data); + -- Response/Suppression Time 2/2 + when 6 => + mem_endpoint_data_next.res_time(1) <= unsigned(mem_read_data); + -- Next Sequence Number 1/2 + when 7 => + mem_endpoint_data_next.next_seq_nr(0) <= unsigned(mem_read_data); + -- Next Sequence Number 2/2 + when 8 => + mem_endpoint_data_next.next_seq_nr(1) <= unsigned(mem_read_data); + -- Highest Available Sequence Number 1/2 + when 9 => + mem_endpoint_data_next.highest_seq_nr(0) <= unsigned(mem_read_data); + -- Highest Available Sequence Number 2/2 + when 10 => + mem_endpoint_data_next.highest_seq_nr(1) <= unsigned(mem_read_data); + -- DONE + mem_stage_next <= IDLE; + when others => + null; + end case; + when INSERT_ENDPOINT => + mem_wr <= '1'; + mem_addr_next <= mem_addr + 1; + mem_cnt_next <= mem_cnt + 1; + + case (mem_cnt) is + -- Entity ID + when 0 => + mem_write_data <= mem_ctrl_data.guid(3); + -- GUID Prefix 1/3 + when 1 => + mem_write_data <= mem_ctrl_data.guid(0); + -- GUID Prefix 2/3 + when 2 => + mem_write_data <= mem_ctrl_data.guid(1); + -- GUID Prefix 3/3 + when 3 => + mem_write_data <= mem_ctrl_data.guid(2); + -- IPv4 Address + when 4 => + mem_write_data <= mem_ctrl_data.addr; + -- UDPv4 Ports + when 5 => + mem_write_data <= mem_ctrl_data.portn & (0 => mem_ctrl_data.expects_inline_qos, 16 downto 1 => '0'); + -- Lease Deadline 1/2 + when 6 => + mem_write_data <= std_logic_vector(mem_ctrl_data.deadline(0)); + -- Lease Deadline 2/2 + when 7 => + mem_write_data <= std_logic_vector(mem_ctrl_data.deadline(1)); + -- Response/Suppression Time 1/2 + when 8 => + mem_write_data <= (others => '0'); + -- Response/Suppression Time 2/2 + when 9 => + mem_write_data <= (others => '0'); + -- Next Sequence Number 1/2 + when 10 => + mem_write_data <= (others => '0'); + -- Next Sequence Number 2/2 + when 11 => + mem_write_data <= to_unsigned(1,CDR_LONG_WIDTH); + -- Highest Available Sequence Number 1/2 + when 12 => + mem_write_data <= (others => '0'); + -- Highest Available Sequence Number 2/2 + when 13 => + mem_write_data <= (others => '0'); + -- DONE + mem_stage_next <= IDLE; + when others => + null; + end case; + when UPDATE_ENDPOINT => + mem_cnt_next <= mem_cnt + 1; + mem_addr_next <= mem_addr + 1; + + case (mem_cnt) is + -- IPv4 Address + when 0 => + mem_write_data <= mem_ctrl_data.addr; + if ((mem_ctrl_data.update_flags and ENDPOINT_DATA_FLAG) = ENDPOINT_DATA_FLAG) then + mem_wr <= '1'; + end if; + -- UDPv4 Ports + when 1 => + mem_write_data <= mem_ctrl_data.portn & (0 => mem_ctrl_data.expects_inline_qos, 16 downto 1 => '0'); + if ((mem_ctrl_data.update_flags and ENDPOINT_DATA_FLAG) = ENDPOINT_DATA_FLAG) then + mem_wr <= '1'; + end if; + -- If nothing else to update + if ((mem_ctrl_data.update_flags and (LEASE_DEADLINE_FLAG or NEXT_SEQ_NR_FLAG or HIGHEST_SEQ_NR_FLAG or RES_TIME_FLAG)) = (mem_ctrl_data.update_flags'range => '0')) then + -- DONE + mem_stage_next <= IDLE; + end if; + -- Lease Deadline 1/2 + when 2 => + mem_write_data <= std_logic_vector(mem_ctrl_data.deadline(0)); + if ((mem_ctrl_data.update_flags and LEASE_DEADLINE_FLAG) = LEASE_DEADLINE_FLAG) then + mem_wr <= '1'; + end if; + -- Lease Deadline 2/2 + when 3 => + mem_write_data <= std_logic_vector(mem_ctrl_data.deadline(1)); + if ((mem_ctrl_data.update_flags and LEASE_DEADLINE_FLAG) = LEASE_DEADLINE_FLAG) then + mem_wr <= '1'; + end if; + -- If nothing else to update + if ((mem_ctrl_data.update_flags and (NEXT_SEQ_NR_FLAG or HIGHEST_SEQ_NR_FLAG or RES_TIME_FLAG)) = (mem_ctrl_data.update_flags'range => '0')) then + -- DONE + mem_stage_next <= IDLE; + end if; + -- Response/Suppression Time 1/2 + when 4 => + mem_write_data <= std_logic_vector(mem_ctrl_data.deadline(0)); + if ((mem_ctrl_data.update_flags and RES_TIME_FLAG) = RES_TIME_FLAG) then + mem_wr <= '1'; + end if; + -- Response/Suppression Time 2/2 + when 5 => + mem_write_data <= std_logic_vector(mem_ctrl_data.deadline(1)); + if ((mem_ctrl_data.update_flags and RES_TIME_FLAG) = RES_TIME_FLAG) then + mem_wr <= '1'; + end if; + -- If nothing else to update + if ((mem_ctrl_data.update_flags and (HIGHEST_SEQ_NR_FLAG or NEXT_SEQ_NR_FLAG)) = (mem_ctrl_data.update_flags'range => '0')) then + -- DONE + mem_stage_next <= IDLE; + end if; + -- Next Sequence Number 1/2 + when 6 => + mem_write_data <= std_logic_vector(mem_ctrl_data.next_seq_nr(0)); + if ((mem_ctrl_data.update_flags and NEXT_SEQ_NR_FLAG) = NEXT_SEQ_NR_FLAG) then + mem_wr <= '1'; + end if; + -- Next Sequence Number 2/2 + when 7 => + mem_write_data <= std_logic_vector(mem_ctrl_data.next_seq_nr(1)); + if ((mem_ctrl_data.update_flags and NEXT_SEQ_NR_FLAG) = NEXT_SEQ_NR_FLAG) then + mem_wr <= '1'; + end if; + -- If nothing else to update + if ((mem_ctrl_data.update_flags and (HIGHEST_SEQ_NR_FLAG)) = (mem_ctrl_data.update_flags'range => '0')) then + -- DONE + mem_stage_next <= IDLE; + end if; + -- Highest Available Sequence Number 1/2 + when 8 => + mem_write_data <= std_logic_vector(mem_ctrl_data.highest_seq_nr(0)); + if ((mem_ctrl_data.update_flags and HIGHEST_SEQ_NR_FLAG) = HIGHEST_SEQ_NR_FLAG) then + mem_wr <= '1'; + end if; + -- Highest Available Sequence Number 2/2 + when 9 => + mem_write_data <= std_logic_vector(mem_ctrl_data.highest_seq_nr(1)); + if ((mem_ctrl_data.update_flags and HIGHEST_SEQ_NR_FLAG) = HIGHEST_SEQ_NR_FLAG) then + mem_wr <= '1'; + end if; + -- DONE + mem_stage_next <= IDLE; + when others => + null; + end case; + when REMOVE_ENDPOINT => + -- Mark with ENTITYID_UNKNOWN to mark slot empty + mem_wr <= '1'; + mem_write_data <= ENTITYID_UNKNOWN; + + if (mem_ctrl_data.mem_opcode = REMOVE_PARTICIPANT) then + -- Continue Participant Match Search + mem_addr_base_next <= addr_res; + mem_addr_next <= addr_res; + mem_stage_next <= SEARCH_ENDPOINT; + mem_cnt_next <= 0; + else + -- Reset MAX Endpoint Pointer + mem_addr_base_next <= FIRST_ENDPOINT_ADDRESS; + mem_addr_next <= FIRST_ENDPOINT_ADDRESS; + reset_max_pointer_next <= '1'; + last_addr_next <= (others => '0'); + mem_stage_next <= FIND_ENDPOINT_SLOT; + mem_cnt_next <= 0; + end if; + when FIND_ENDPOINT_SLOT => + mem_rd <= '1'; + mem_addr_next <= mem_addr + 1; + mem_cnt_next <= mem_cnt + 1; + + -- Next Endpoint Frame Address + tmp := mem_addr_base + ENDPOINT_FRAME_SIZE; + + case (mem_cnt) is + -- Preload + when 0 => + null; + -- Entity ID + when 1 => + -- Slot Occupied + if (mem_read_data /= ENTITYID_UNKNOWN) then + -- Reached end of Endpoint Memory Area + if (mem_addr_base = max_endpoint_addr) then + -- We are in the middle of resetting the MAX Endpoint Pointer + if (reset_max_pointer = '1') then + -- No Change + mem_stage_next <= IDLE; + -- MEMORY FULL + elsif (max_endpoint_addr = MAX_ENDPOINT_ADDRESS) then + report "Memory Full, Ignoring Endpoint Data" severity NOTE; + -- Ignore Insertion + mem_stage_next <= IDLE; + addr_res_next <= MAX_ADDRESS; + else + -- Extend Endpoint Memory Area + -- NOTE: "max_endpoint_addr" points to the first address of last Endpoint Frame + max_endpoint_addr_next <= tmp; + -- Populate Endpoint Slot + mem_stage_next <= INSERT_ENDPOINT; + mem_addr_next <= tmp; + addr_res_next <= tmp; + mem_cnt_next <= 0; + end if; + else + -- Latch last occupied Endpoint Slot + last_addr_next <= mem_addr_base; + -- Continue Search + mem_addr_next <= tmp; + mem_addr_base_next <= tmp; + mem_cnt_next <= 0; + end if; + -- Slot Empty + else + if (reset_max_pointer = '1') then + -- Make sure to iterate through complete Endpoint Area + if (mem_addr_base = max_endpoint_addr) then + -- Reset Pointer to last occupied Endpoint Slot + max_endpoint_addr_next <= last_addr; + -- DONE + mem_stage_next <= IDLE; + else + -- Continue Search + mem_addr_next <= tmp; + mem_addr_base_next <= tmp; + mem_cnt_next <= 0; + end if; + else + -- Populate Endpoint Slot + mem_stage_next <= INSERT_ENDPOINT; + mem_addr_next <= mem_addr_base; + addr_res_next <= mem_addr_base; + mem_cnt_next <= 0; + end if; + end if; + when others => + null; + end case; + when FIND_NEXT_ENDPOINT => + mem_rd <= '1'; + mem_cnt_next <= mem_cnt + 1; + mem_addr_next <= mem_addr + 1; + + -- Next Endpoint Frame Address + tmp := mem_addr_base + ENDPOINT_FRAME_SIZE; + -- Beginning of Endpoint Data + tmp2 := mem_addr_base + 4; + + case (mem_cnt) is + -- Preload + when 0 => + null; + -- Entity ID + when 1 => + -- Slot Occupied + if (mem_read_data /= ENTITYID_UNKNOWN) then + -- Get Endpoint Data + mem_addr_next <= tmp2; + addr_res_next <= mem_addr_base; + mem_stage_next <= GET_ENDPOINT_DATA; + mem_cnt_next <= 0; + -- Slot Empty + else + -- Reached End of Memory, No Match + if (mem_addr_base = max_endpoint_addr) then + addr_res_next <= MAX_ADDRESS; --No match + -- DONE + mem_stage_next <= IDLE; + else + -- Continue Search + mem_addr_next <= tmp; + mem_addr_base_next <= tmp; + mem_cnt_next <= 0; + end if; + end if; + when others => + null; + end case; + when FIND_STALE_ENDPOINT => + mem_rd <= '1'; + mem_cnt_next <= mem_cnt + 1; + mem_addr_next <= mem_addr + 1; + + -- Next Endpoint Frame Address + tmp := mem_addr_base + ENDPOINT_FRAME_SIZE; + -- Beginning of Endpoint Data + tmp2 := mem_addr_base + 4; + -- Beginning of Lease Deadline + tmp3 := mem_addr_base + 6; + + case (mem_cnt) is + -- Preload + when 0 => + null; + -- Entity ID + when 1 => + -- Slot Occupied + if (mem_read_data /= ENTITYID_UNKNOWN) then + -- Jump to Stale Check + mem_addr_next <= tmp3; + mem_cnt_next <= 2; + -- Slot Empty + else + -- Reached End of Memory, No Match + if (mem_addr_base = max_endpoint_addr) then + addr_res_next <= MAX_ADDRESS; --No match + -- DONE + mem_stage_next <= IDLE; + else + -- Continue Search + mem_addr_next <= tmp; + mem_addr_base_next <= tmp; + mem_cnt_next <= 0; + end if; + end if; + -- Preload + when 2 => + null; + -- Lease Deadline 1/2 + when 3 => + mem_long_latch_next <= mem_read_data; + -- Lease Deadline 2/2 + when 4 => + tmp_dw := (0 => unsigned(mem_long_latch), 1 => unsigned(mem_read_data)); + -- Lease Deadline passed + if (tmp_dw < time) then + -- Mark Endpoint as stale + addr_res_next <= mem_addr_base; + mem_endpoint_data_next <= ZERO_ENDPOINT_DATA; + -- DONE + mem_stage_next <= IDLE; + end if; + -- Response/Suppression Time 1/2 + when 5 => + mem_long_latch_next <= mem_read_data; + -- Response/Suppression Time 2/2 + when 6 => + tmp_dw := (0 => unsigned(mem_long_latch), 1 => unsigned(mem_read_data)); + -- Response/Suppression Time passed + if (tmp_dw /= 0 and tmp_dw < time) then + -- Mark Endpoint and get Endpoint Data + addr_res_next <= mem_addr_base; + mem_addr_next <= tmp2; + mem_stage_next <= GET_ENDPOINT_DATA; + mem_cnt_next <= 0; + -- Endpoint not Stale + else + -- Reached End of Memory, No Match + if (mem_addr_base = max_endpoint_addr) then + addr_res_next <= MAX_ADDRESS; --No match + -- DONE + mem_stage_next <= IDLE; + else + -- Continue Search + mem_addr_next <= tmp; + mem_addr_base_next <= tmp; + mem_cnt_next <= 0; + end if; + end if; + when others => + null; + end case; + 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 <= (others => '0'); + -- Increment read counter each time rd is high + elsif (rd_sig = '1') then + read_cnt <= read_cnt + 1; + end if; + end if; + end process; + +end architecture; diff --git a/src/rtps_package.vhd b/src/rtps_package.vhd index f62b6c4..1640b32 100644 --- a/src/rtps_package.vhd +++ b/src/rtps_package.vhd @@ -47,6 +47,8 @@ package rtps_package is constant COUNT_WIDTH : natural := CDR_LONG_WIDTH; constant SUBMESSAGE_DATA_EXTRA_FLAGS_WIDTH : natural := 16; constant MAX_BITMAP_WIDTH : natural := 256; + constant KEY_HASH_WIDTH : natural := 128; + constant STATUS_INFO_WIDTH : natural := 32; -- *TYPES DEFINITION* -- Generic Types @@ -59,6 +61,7 @@ package rtps_package is type GUIDPREFIX_TYPE is array (0 to (GUIDPREFIX_WIDTH/WORD_WIDTH)-1) of std_logic_vector(WORD_WIDTH-1 downto 0); type GUID_TYPE is array (0 to (GUID_WIDTH/WORD_WIDTH)-1) of std_logic_vector(WORD_WIDTH-1 downto 0); type BITMAP_TYPE is array (0 to (MAX_BITMAP_WIDTH/WORD_WIDTH)-1) of std_logic_vector(0 to WORD_WIDTH-1); + type KEY_HASH_TYPE is array (0 to (KEY_HASH_WIDTH/WORD_WIDTH)-1) of std_logic_vector(WORD_WIDTH-1 downto 0); -- Helper Function function gen_duration(s,ns : integer) return DURATION_TYPE;