greek64/rtps-fpga
2
1
Fork 0
Code Issues Pull Requests Releases Wiki Activity

* Finish Memory FSM

Browse Source
This commit is contained in:
John Ring 2020-10-06 08:35:31 +02:00
parent e6e4094583
commit aed60f9b01
3 changed files with 527 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/TODO.txt
View File

@ -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 +

559
src/rtps_builtin_endpoint.vhd
View File

@ -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;

5
src/rtps_package.vhd
View File

@ -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);
--****************