diff --git a/src/TODO.txt b/src/TODO.txt index fc79147..fd56659 100644 --- a/src/TODO.txt +++ b/src/TODO.txt @@ -123,15 +123,15 @@ PARTICICPANT DATA +---------------------------------------------------------------+ 06| META_UDP_PORT | DEFAULT_UDP_PORT | +---------------------------------------------------------------+ - | UNUSED | EXTRA_FLAGS |Q| +07| UNUSED | EXTRA_FLAGS |Q| +---------------------------------------------------------------+ -07| LEASE_DURATION | +08| LEASE_DURATION | + + -08| | +09| | +---------------------------------------------------------------+ -09| LEASE_DEADLINE | +10| LEASE_DEADLINE | + + -10| | +11| | +---------------------------------------------------------------+ 12| | + SPDP_SEQ_NR + diff --git a/src/rtps_builtin_endpoint.vhd b/src/rtps_builtin_endpoint.vhd index 3d38390..f2d504e 100644 --- a/src/rtps_builtin_endpoint.vhd +++ b/src/rtps_builtin_endpoint.vhd @@ -5,9 +5,9 @@ use ieee.numeric_std.all; use work.math_pkg.all; use work.rtps_package.all; --- Checksum has to be checked before +-- XXX: The AUTO_PURGE could return through the FIND_ORPHAN_ENDPOINT branch, which returns to IDLE through the SKIP_PACKET stage. Could it be, that a entire packet is droped due to this, or is the "read_counter" and "packet_length" set to prevent that? -entity rtps_handler is +entity rtps_builtin_endpoint is generic ( DOMAIN_ID : integer := 0; DOMAIN_TAG : std_logic_vector(0 to (256*8)-1) := (others => '0') @@ -24,7 +24,7 @@ entity rtps_handler is ); end entity; -architecture arch of rtps_handler is +architecture arch of rtps_builtin_endpoint is --*****COMPONENT DECLARATION****** component single_port_ram is @@ -89,6 +89,7 @@ architecture arch of rtps_handler is 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 lease_deadline : DURATION_TYPE := (others => (others => '0')); signal lifespan_duration, lifespan_duration_next : DURATION_TYPE := (others => (others => '0')); signal addr_latch_index, addr_latch_index_next : std_logic := '0'; -- General Purpose counter @@ -101,6 +102,7 @@ architecture arch of rtps_handler is 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 last_addr, last_addr_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'); @@ -117,6 +119,9 @@ architecture arch of rtps_handler is 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'); + signal reset_max_pointer, reset_max_pointer_next : std_logic := '0'; + signal stale_check, stale_check_next : std_logic := '0'; + signal mem_guidprefix, mem_guidprefix_next : GUIDPREFIX_ARRAY_TYPE : (others => (others => '0')); --*****ALIAS DEFINATION***** @@ -209,6 +214,15 @@ begin end if; end process; + lease_deadline_prc : process(all) + variable tmp : std_logic_vector(63 downto 0) := (others => '0'); + begin + tmp := lease_duration(0) & lease_duration(1); + tmp := tmp + TODO; + lease_deadline(0) <= tmp(63 downto 32); + lease_deadline(1) <= tmp(31 downto 0); + end process; + -- This process connects the Intermediate Output Signals to the actual output FIFOs output_prc : process(all) begin @@ -269,12 +283,30 @@ begin participant_message_best_effort_next <= participant_message_best_effort; output_sig <= (others => '0'); wr_sig <= '0'; + stale_check_next <= stale_check; -- TODO: Reset Latches case(stage) is -- Initial/Idle State - when INIT => + when IDLE => + -- No Packets to process + if (empty = '1') then + mem_opcode <= FIND_STALE_PARTICIPANT; + start_mem_op <= '1'; + stale_check_next <= '1'; + stage_next <= AUTO_PURGE; + elsif (stale_check = '0') then + mem_opcode <= FIND_STALE_PARTICIPANT; + start_mem_op <= '1'; + stale_check_next <= '1'; + stage_next <= AUTO_PURGE; + else + -- Process Packet + stale_check_next <= '0'; + stage_next <= PACKET_LENGTH; + end if; + when PACKET_LENGTH => if (empty = '0') then rd_sig <= '1'; -- Latch Packet Length @@ -1204,7 +1236,7 @@ begin -- DEFAULT Next Stage stage_next <= SKIP_PARAMETER; end if; - when PARTICIPANT_MATCH_STAGE => + when PARTICIPANT_MATCH_STAGE_1 => -- Wait for Participant Search if (mem_op_done = '1') then -- No Match in Buffer @@ -1222,21 +1254,28 @@ begin 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'; + -- Participant + if (message_type = PDP) then + -- Participant Unmatch + if (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; + else + -- Store new Sequence Number + mem_opcode <= UPDATE_PARTICIPANT; + start_mem_op <= '1'; + -- DONE + stage_next <= SKIP_PACKET; + end if; -- Endpoint elsif (message_type = EDP) then - -- Search Endpoint in Buffer - mem_opcode <= SEARCH_ENDPOINT; + -- Store new Sequence Number + mem_opcode <= UPDATE_PARTICIPANT; start_mem_op <= '1'; - -- DONE - stage_next <= ENDPOINT_MATCH_STAGE; + stage_next <= INITIATE_ENDPOINT_SEARCH; else -- Ignore stage_next <= SKIP_PACKET; @@ -1246,6 +1285,15 @@ begin stage_next <= SKIP_PACKET; end if; end if; + -- Help stage, because we need to do two consecutive memory operations + when INITIATE_ENDPOINT_SEARCH => + if (mem_op_done = '1') then + -- Search Endpoint in Buffer + mem_opcode <= SEARCH_ENDPOINT; + start_mem_op <= '1'; + -- DONE + stage_next <= ENDPOINT_MATCH_STAGE; + end if; when ENDPOINT_MATCH_STAGE => -- Wait for Endpoint Search if (mem_op_done = '1') then @@ -1267,7 +1315,7 @@ begin 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); + tmp_endpoint_mask := convert_from_bitmask_array(endpoint_mask_array, MAX_ENDPOINTS); -- Mark Endpoint match changes tmp_endpoint_mask := tmp_endpoint_mask xor endpoint_mask; -- Mark UNMATCHES @@ -1286,7 +1334,7 @@ begin start_mem_op <= '1'; -- Mark UNMATCHES endpoint_match_next <= (others => '0'); - endpoint_unmatch_next <= convert_from_bitmask_array(endpoint_mask_array); + endpoint_unmatch_next <= convert_from_bitmask_array(endpoint_mask_array, MAX_ENDPOINTS); -- Propagate Match Changes to local Endpoints stage_next <= INFORM_ENDPOINTS_UNMATCH; cnt_next <= 1; @@ -1295,9 +1343,10 @@ begin 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; + is_orphan_search_next <= '1'; + 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 @@ -1308,7 +1357,7 @@ begin start_mem_op <= '1'; -- Mark UNMATCHES endpoint_match_next <= (others => '0'); - endpoint_unmatch_next <= convert_from_bitmask_array(endpoint_mask_array); + endpoint_unmatch_next <= convert_from_bitmask_array(endpoint_mask_array, MAX_ENDPOINTS); -- Propagate Match Changes to local Endpoints stage_next <= INFORM_ENDPOINTS_UNMATCH; cnt_next <= 1; @@ -1388,29 +1437,49 @@ begin end if; end case; end if; + when AUTO_PURGE_1 => + -- Wait for memory OP + if (mem_op_done = '1') then + -- Found Stale Entry + if (addr_res /= to_unsigned(BUILTIN_BUFFER_SIZE, addr_res'length)) then + mem_opcode <= REMOVE_PARTICIPANT; + start_mem_op <= '1'; + stage_next <= AUTO_PURGE_2; + else + stage_next <= IDLE; + end if; + end if; + when AUTO_PURGE_2 => + if (mem_op_done = '1') then + -- Help Stage needed to latch the GUID Prefix of the removed staled participant (Needed for the orphan search) + guid_next(0) <= mem_guidprefix(0); + guid_next(1) <= mem_guidprefix(1); + guid_next(2) <= mem_guidprefix(2); + stage_next <= FIND_ORPHAN_ENDPOINT; + end if; --############################# when SKIP_PARAMETER => -- End of Parameter if (read_cnt > parameter_end) then -- Begin parsing of next parameter stage_next <= PROCESS_PL; - else - if (empty = '0') then - rd_sig <= '1'; - if (read_cnt = parameter_end) then - -- Begin parsing of next parameter - stage_next <= PROCESS_PL; - end if; + elsif (empty = '0') then + rd_sig <= '1'; + if (read_cnt = parameter_end) then + -- Begin parsing of next parameter + stage_next <= PROCESS_PL; end if; end if; when SKIP_PACKET => - -- TODO: Handle read_cnt > packet_length (Like above) - if (empty = '0') then + if (read_cnt > packet_length) then + -- Begin parsing of next parameter + stage_next <= PROCESS_PL; + elsif (empty = '0') then rd_sig <= '1'; -- End of Packet if (read_cnt = packet_length) then -- Continue parsing next packet - stage_next <= INIT; + stage_next <= IDLE; end if; end if; when others => @@ -1422,6 +1491,7 @@ begin mem_ctrl_prc : process(all) variable tmp : unsigned(mem_addr_base'range) := (others => '0'); + variable tmp2 : unsigned(mem_addr_base'range) := (others => '0'); begin -- DEFAULT mem_op_done <= '0'; @@ -1437,11 +1507,12 @@ begin mem_def_addr_next <= mem_def_addr; mem_def_port_next <= mem_def_port; mem_xflags_next <= mem_xflags; + last_addr_next <= last_addr; case (mem_stage) is when IDLE => mem_op_done <= '1'; - is_orphan_search_next <= '0'; + reset_max_pointer_next <= '0'; if (start_mem_op = '1') then case(mem_opcode) is @@ -1451,11 +1522,77 @@ begin 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)); + mem_stage_next <= SEARCH_ENDPOINT; + tmp := to_unsigned(BUILTIN_BUFFER_SIZE - ENDPOINT_FRAME_SIZE, mem_addr_base'length); + mem_addr_base_next <= tmp; + mem_addr_next <= tmp; + when REMOVE_PARTICIPANT => + mem_stage_next <= REMOVE_PARTICIPANT; + mem_addr_next <= addr_res; + mem_cnt_next <= 1; + when REMOVE_ENDPOINT => + mem_stage_next <= REMOVE_ENDPOINT; + mem_addr_next <= addr_res; + mem_cnt_next <= 1; + when UPDATE_ENDPOINT => + mem_stage_next <= UPDATE_ENDPOINT; + mem_addr_next <= addr_res + to_unsigned(4, mem_addr_base'length); + mem_cnt_next <= 0; + endpoint_mask_array_next <= convert_to_bitmask_array(endpoint_mask); + when INSERT_PARTICIPANT => + mem_stage_next <= FIND_PARTICIPANT_SLOT; + tmp := (others => '0'); + mem_addr_base_next <= tmp; + mem_addr_next <= tmp; + mem_cnt_next <= 0; + when INSERT_ENDPOINT => + mem_stage_next <= FIND_ENDPOINT_SLOT; + tmp := to_unsigned(BUILTIN_BUFFER_SIZE - ENDPOINT_FRAME_SIZE, mem_addr_base'length); + mem_addr_base_next <= tmp; + mem_addr_next <= tmp; + mem_cnt_next <= 0; + when RESET_MAX_PARTICIPANT_POINTER => + mem_stage_next <= FIND_PARTICIPANT_SLOT; + tmp := (others => '0'); + mem_addr_base_next <= tmp; + mem_addr_next <= tmp; + mem_cnt_next <= 0; + reset_max_pointer_next <= '1'; + last_addr_next <= (others => '0'); + when RESET_MAX_ENDPOINT_POINTER => + mem_stage_next <= FIND_ENDPOINT_SLOT; tmp := to_unsigned(BUILTIN_BUFFER_SIZE - ENDPOINT_FRAME_SIZE, mem_addr_base'length); mem_addr_base_next <= tmp; mem_addr_next <= tmp; + mem_cnt_next <= 0; + reset_max_pointer_next <= '1'; + last_addr_next <= to_unsigned(BUILTIN_BUFFER_SIZE, mem_addr_base'length); + when FIND_STALE_PARTICIPANT => + mem_stage_next <= FIND_STALE_PARTICIPANT; + tmp := (others => '0'); + mem_addr_base_next <= tmp; + mem_addr_next <= tmp; + mem_cnt_next <= 0; + when UPDATE_PARTICIPANT => + mem_stage_next <= UPDATE_PARTICIPANT; + mem_cnt_next <= 1; + case (message_type) is + when PDP => + mem_addr_next <= addr_res + to_unsigned(11, mem_addr'length); + when EDP => + -- Publisher + if (is_requested = '0') then + mem_addr_next <= addr_res + to_unsigned(13, mem_addr'length); + -- Subscriber + else + mem_addr_next <= addr_res + to_unsigned(15, mem_addr'length); + end if; + when MESSAGE => + mem_addr_next <= addr_res + to_unsigned(17, mem_addr'length); + when others => + -- Uknown Message Type. + mem_stage_next <= IDLE; + end case; when others => null; end case; @@ -1468,7 +1605,7 @@ begin -- 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 + 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; @@ -1492,7 +1629,7 @@ begin end if; when 2 => -- No Match - if (mem_read_data /= guid(0)) then + if (mem_read_data /= guid(1)) then -- Continue Search mem_stage_next <= SEARCH_PARTICIPANT; mem_addr_next <= tmp; @@ -1500,7 +1637,7 @@ begin end if; when 3 => -- No Match - if (mem_read_data /= guid(0)) then + if (mem_read_data /= guid(2)) then -- Continue Search mem_stage_next <= SEARCH_PARTICIPANT; mem_addr_next <= tmp; @@ -1581,7 +1718,7 @@ begin mem_stage_next <= COMPARE_GUID; -- Reached MAX Addr, No Match Found - if (mem_addr_h <= max_endpoint_addr) then + if (mem_addr_base = max_endpoint_addr) then addr_res_next <= to_unsigned(BUILTIN_BUFFER_SIZE, addr_res'length);; --No match mem_stage_next <= IDLE; end if; @@ -1650,7 +1787,349 @@ begin -- DONE mem_stage_next <= IDLE; end if; + when REMOVE_PARTICIPANT => + -- Default Address Increment + mem_addr_next <= mem_addr + to_unsigned(1, mem_addr'length); + -- Increment counter + mem_cnt_next <= mem_cnt + 1; + + -- Latch Participant GUID Prefix, and then overwrite with GUIDPREFIX_UNKNOWN to mark as empty + case (mem_cnt) is + when 0 => + -- Preload + mem_rd <= '1'; + when 1 => + mem_rd <= '1'; + mem_guidprefix_next(0) <= mem_read_data; + when 2 => + mem_rd <= '1'; + mem_guidprefix_next(1) <= mem_read_data; + when 3 => + mem_rd <= '1'; + mem_guidprefix_next(2) <= mem_read_data; + mem_addr_next <= addr_res; + when 4 => + mem_wr <= '1'; + mem_write_data <= GUIDPREFIX_UNKNOWN_ARRAY(0); + when 5 => + mem_wr <= '1'; + mem_write_data <= GUIDPREFIX_UNKNOWN_ARRAY(1); + when 6 => + mem_wr <= '1'; + mem_write_data <= GUIDPREFIX_UNKNOWN_ARRAY(2); + -- DONE + mem_stage_next <= RESET_MAX_PARTICIPANT_POINTER; + when others => + null; + end case; + when REMOVE_ENDPOINT => + mem_wr <= '1'; + + mem_write_data <= ENTITYID_UNKNOWN; + -- DONE + mem_stage_next <= RESET_MAX_ENDPOINT_POINTER; + when UPDATE_ENDPOINT => + mem_wr <= '1'; + -- Default Address Increment + mem_addr_next <= mem_addr + to_unsigned(1, mem_addr'length); + -- Increment counter + mem_cnt_next <= mem_cnt + 1; + + mem_write_data <= endpoint_mask_array(mem_cnt); + -- Exit Condition + if (mem_cnt = ENDPOINT_BITMASK_SIZE-1) then + -- DONE + mem_stage_next <= IDLE; + end if; + when FIND_PARTICIPANT_SLOT => + mem_rd <= '1'; + -- Default Address Increment + mem_addr_next <= mem_addr + to_unsigned(1, mem_addr'length); + -- Increment counter + mem_cnt_next <= mem_cnt + 1; + -- Next Participant Frame Address + tmp := mem_addr_base + to_unsigned(PARTICIPANT_FRAME_SIZE, mem_addr_base'length); + + case (mem_cnt) is + when 0 => + -- Preload + -- Reached MAX Addr + if (mem_addr_base = max_participant_addr) then + if (reset_max_pointer = '1') then + -- Reset "max_participant_addr" to first free slot after last occupied slot + if (last_addr /= (last_addr'reverse_range => '0')) then + max_participant_addr_next <= last_addr; + end if; + -- DONE (reset pointer) + mem_stage_next <= IDLE; + -- MEMORY COLLISION + -- XXX: Posible worst case path (addition and comparison same clock) + elsif (tmp > max_endpoint_addr) then + -- Ignore Insertion + mem_stage_next <= IDLE; + else + -- Extend Participant Memory Area + -- NOTE: "max_participant_addr" points to the first address after the last participant frame + max_participant_addr_next <= tmp; + -- DONE + mem_stage_next <= INSERT_PARTICIPANT; + mem_cnt_next <= 1; + end if; + end if; + when 1 => + if (mem_read_data /= GUIDPREFIX_UNKNOWN_ARRAY(0)) then + mem_addr_next <= tmp; + mem_addr_base_next <= tmp; + mem_cnt_next <= 0; + last_addr_next <= (others => '0'); + end if; + when 2 => + if (mem_read_data /= GUIDPREFIX_UNKNOWN_ARRAY(1)) then + mem_addr_next <= tmp; + mem_addr_base_next <= tmp; + mem_cnt_next <= 0; + last_addr_next <= (others => '0'); + end if; + when 3 => + if (mem_read_data /= GUIDPREFIX_UNKNOWN_ARRAY(2)) then + mem_addr_next <= tmp; + mem_addr_base_next <= tmp; + mem_cnt_next <= 0; + last_addr_next <= (others => '0'); + else + -- If "reset_max_pointer" is set, go through all the participant memory area to reset the pointer + if (reset_max_pointer = '1') then + -- Max pointer reset logic + -- Store first free slot address after occupied slot + if (last_addr /= (last_addr'reverse_range => '0')) then + last_addr_next <= mem_addr_base; + end if; + else + -- Found Empty Slot, DONE + mem_stage_next <= INSERT_PARTICIPANT; + mem_cnt_next <= 1; + end if; + end if; + end case; + when INSERT_PARTICIPANT => + mem_wr <= '1'; + -- Default Address Increment + mem_addr_next <= mem_addr + to_unsigned(1, mem_addr'length); + -- Increment counter + mem_cnt_next <= mem_cnt + 1; + + case (mem_cnt) is + when 1 => + -- GUIDPrefix 1/3 + mem_write_data <= guid(0); + when 2 => + -- GUIDPrefix 2/3 + mem_write_data <= guid(1); + when 3 => + -- GUIDPrefix 3/3 + mem_write_data <= guid(2); + when 4 => + -- Metatraffic IPv4 Address + mem_write_data <= addr_latch_2; + when 5 => + -- Default Endpoint IPv4 Address + mem_write_data <= addr_latch_1; + when 6 => + -- UDPv4 Ports + mem_write_data <= port_latch_2 & port_latch_1; + when 7 => + -- Extra Flags + mem_write_data <= (0 => expects_inline_qos, others => '0'); + when 8 => + -- Lease Duration 1/2 + mem_write_data <= lease_duration(0); + when 9 => + -- Lease Duration 2/2 + mem_write_data <= lease_duration(1); + when 10 => + -- Lease Deadline 1/2 + mem_write_data <= lease_deadline(0); + when 11 => + -- Lease Deadline 2/2 + mem_write_data <= lease_deadline(1); + when 12 => + -- SPDP Sequence Number 1/2 + mem_write_data <= seq_nr(0); + when 13 => + -- SPDP Sequence Number 2/2 + mem_write_data <= seq_nr(1); + when 14 => + -- Publication Sequence Number 1/2 + mem_write_data <= (others => '0'); + when 15 => + -- Publication Sequence Number 2/2 + mem_write_data <= (others => '0'); + when 16 => + -- Subscription Sequence Number 1/2 + mem_write_data <= (others => '0'); + when 17 => + -- Subscription Sequence Number 2/2 + mem_write_data <= (others => '0'); + when 18 => + -- Participant Message Sequence Number 1/2 + mem_write_data <= (others => '0'); + when 19 => + -- Participant Message Sequence Number 2/2 + mem_write_data <= (others => '0'); + end case; + when FIND_ENDPOINT_SLOT => + mem_rd <= '1'; + -- Increment counter + mem_cnt_next <= mem_cnt + 1; + -- Next Endpoint Frame Address + tmp := mem_addr_base - to_unsigned(ENDPOINT_FRAME_SIZE, mem_addr_base'length); + + case (mem_cnt) is + when 0 => + -- Preload + -- Exceeded MAX Addr + if (mem_addr_base > max_endpoint_addr) then + if (reset_max_pointer = '1') then + -- Reset "max_endpoint_addr" to last occupied slot + if (last_addr /= to_unsigned(BUILTIN_BUFFER_SIZE, mem_addr_base'length)) then + max_endpoint_addr_next <= last_addr; + end if; + -- DONE (reset pointer) + mem_stage_next <= IDLE; + -- MEMORY COLLISION + elsif (mem_addr_base < max_participant_addr) then + -- Ignore Insertion + mem_stage_next <= IDLE; + else + -- Extend Participant Memory Area + -- NOTE: "max_endpoint_addr" points to the beginning of the last endpoint frame + max_endpoint_addr <= mem_addr_base; + -- DONE + mem_stage_next <= INSERT_PARTICIPANT; + mem_cnt_next <= 1; + end if; + end if; + when 1 => + if (mem_read_data /= ENTITYID_UNKNOWN) then + mem_addr_next <= tmp; + mem_addr_base_next <= tmp; + mem_cnt_next <= 0; + -- Store last occupied endpoint slot + last_addr_next <= mem_addr_base; + else + -- If "reset_max_pointer" is set, go through all the endpoint memory area to reset the pointer + if (reset_max_pointer = '0') then + -- Found Empty Slot, DONE + mem_stage_next <= INSERT_ENDPOINT; + mem_cnt_next <= 1; + end if; + end if; + end case; + when INSERT_ENDPOINT => + mem_wr <= '1'; + -- Default Address Increment + mem_addr_next <= mem_addr + to_unsigned(1, mem_addr'length); + -- Increment counter + mem_cnt_next <= mem_cnt + 1; + + case (mem_cnt) is + when 1 => + -- Entity ID + mem_write_data <= guid(3); + when 2 => + -- GUIDPrefix 1/3 + mem_write_data <= guid(0); + when 3 => + -- GUIDPrefix 2/3 + mem_write_data <= guid(1); + when 4 => + -- GUIDPrefix 3/3 + mem_write_data <= guid(2); + + -- Write endpoint bitmask via update method + mem_stage_next <= UPDATE_ENDPOINT; + mem_cnt_next <= 0; + endpoint_mask_array_next <= convert_to_bitmask_array(endpoint_mask); + end case; + when FIND_STALE_PARTICIPANT => + 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(PARTICIPANT_FRAME_SIZE, mem_addr_base'length); + tmp2 := mem_addr_base + to_unsigned(7, mem_addr_base'length); + + case (mem_cnt) is + when 0 => + -- Preload + -- 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 1 => + -- If slot occupied, jump to stale check + if (mem_read_data /= GUIDPREFIX_UNKNOWN_ARRAY(0)) then + mem_addr_next <= tmp2; + mem_cnt_next <= 4; + end if; + when 2 => + -- If slot occupied, jump to stale check + if (mem_read_data /= GUIDPREFIX_UNKNOWN_ARRAY(1)) then + mem_addr_next <= tmp2; + mem_cnt_next <= 4; + end if; + when 3 => + -- If slot occupied, jump to stale check + if (mem_read_data /= GUIDPREFIX_UNKNOWN_ARRAY(2)) then + mem_addr_next <= tmp2; + mem_cnt_next <= 4; + -- Slot empty, check next slot + else + mem_addr_next <= tmp; + mem_addr_base_next <= tmp; + mem_cnt_next <= 0; + end if; + when 4 => + -- Preload + null; + when 5 => + -- Deadline passed, mark participant + if (mem_read_data < TODO) then + addr_res_next <= mem_addr_base; + -- DONE + mem_stage_next <= IDLE; + end if; + when 6 => + -- Deadline passed, mark participant + if (mem_read_data < TODO) then + addr_res_next <= mem_addr_base; + -- DONE + mem_stage_next <= IDLE; + -- Lease not expired, check next slot + else + mem_addr_next <= tmp; + mem_addr_base_next <= tmp; + mem_cnt_next <= 0; + end if; + end case; + when UPDATE_PARTICIPANT => + mem_wr <= '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 => + mem_write_data <= seq_nr(63 downto 32); + when 1 => + mem_write_data <= seq_nr(31 downto 0); + mem_stage_next <= IDLE; + end case; when others => + null; end case; end process; diff --git a/src/rtps_package.vhd b/src/rtps_package.vhd index 776bc62..d63e12b 100644 --- a/src/rtps_package.vhd +++ b/src/rtps_package.vhd @@ -262,6 +262,7 @@ package rtps_package is 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(0 to GUIDPREFIX_WIDTH-1) := (others => '0'); + constant GUIDPREFIX_UNKNOWN_ARRAY : GUIDPREFIX_ARRAY_TYPE := (others => (others => '0')); subtype ENTITY_KIND_H is std_logic_vector(1 downto 0); subtype ENTITY_KIND_L is std_logic_vector(5 downto 0); @@ -316,9 +317,9 @@ package rtps_package is constant BEST_EFFORT_PARTICIPANT_MESSAGE_DATA_READER : integer := 0; --*****CUSTOM***** - constant PARTICIPANT_FRAME_SIZE : integer := 12; + constant PARTICIPANT_FRAME_SIZE : integer := 19; constant ENDPOINT_BITMASK_SIZE : integer := round_div(MAX_ENDPOINTS, 32); - constant ENDPOINT_FRAME_SIZE : integer := 2 + ENDPOINT_BITMASK_SIZE; + constant ENDPOINT_FRAME_SIZE : integer := 4 + ENDPOINT_BITMASK_SIZE; -- Limit Buffer to 32 bit Addresses constant BUILTIN_BUFFER_SIZE : integer := min(MAX_ENDPOINTS*PARTICIPANT_FRAME_SIZE, 2**32); --****************