diff --git a/src/TODO.txt b/src/TODO.txt index 7531d65..40e7703 100644 --- a/src/TODO.txt +++ b/src/TODO.txt @@ -63,36 +63,36 @@ The elements of an array are in order (but the elements themselves may need to b ENDPOINT FIFO PACKET FORMAT =========================== -32..............24..............16..............8...............0 -+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ -+---------------+---------------+---------------+---------------+ -| OPCODE | FLAGS | SRC_UDP_PORT | -+---------------+---------------+---------------+---------------+ -| SRC_IPv4_ADDR | -+---------------------------------------------------------------+ -| SRC_ENTITYID | -+---------------------------------------------------------------+ -| | -+ + -| SRC_GUIDPREFIX | -+ + -| | -+---------------------------------------------------------------+ -| DEST_ENTITYID [only for Builtin Destinations] | -+---------------------------------------------------------------+ -| | -+ Sequence Number [only for DATA Submessage] + -| | -+---------------------------------------------------------------+ -| | -~ PAYLOAD (SUBMESSAGE CONTENT) ~ -| | -+---------------------------------------------------------------+ +31............24..............16..............8...............0 ++-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ ++-------------+---------------+---------------+---------------+ +| OPCODE | FLAGS | SRC_UDP_PORT | ++-------------+---------------+---------------+---------------+ +| SRC_IPv4_ADDR | ++-------------------------------------------------------------+ +| SRC_ENTITYID | ++-------------------------------------------------------------+ +| | ++ + +| SRC_GUIDPREFIX | ++ + +| | ++-------------------------------------------------------------+ +| DEST_ENTITYID [only for Builtin Destinations] | ++-------------------------------------------------------------+ +| | ++ Sequence Number [only for DATA Submessage] + +| | ++-------------------------------------------------------------+ +| | +~ PAYLOAD (SUBMESSAGE CONTENT) ~ +| | ++-------------------------------------------------------------+ ENDPOINT_ID =========== -MSB...........LSB +0...MAX_ENDPOINTS READERS...WRITERS BUILT-IN ENDPOINTS @@ -107,124 +107,140 @@ HEARTBEAT messages (and even ignore GAP messages). PARTICICPANT DATA ================= - 32..............24..............16..............8...............0 - +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - +---------------------------------------------------------------+ -01| | - + + -02| GUIDPREFIX | - + + -03| | - +---------------------------------------------------------------+ -04| META_IPv4_ADDRESS | - +---------------------------------------------------------------+ -05| DEFAULT_IPv4_ADDRESS | - +---------------------------------------------------------------+ -06| META_UDP_PORT | DEFAULT_UDP_PORT | - +---------------------------------------------------------------+ -07| UNUSED | EXTRA_FLAGS |Q| - +---------------------------------------------------------------+ -08| LEASE_DURATION | - + + -09| | - +---------------------------------------------------------------+ -10| LEASE_DEADLINE | - + + -11| | - +---------------------------------------------------------------+ -12| | - + SPDP_SEQ_NR + -13| | - +---------------------------------------------------------------+ -14| | - + PUBLICATION_SEQ_NR + -15| | - +---------------------------------------------------------------+ -16| | - + SUBSCRIPTION_SEQ_NR + -17| | - +---------------------------------------------------------------+ -18| | - + MESSAGE_SEQ_NR + -19| | - +---------------------------------------------------------------+ + 31............24..............16..............8...............0 + +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + +-------------------------------------------------------------+ +01| | + + + +02| GUIDPREFIX | + + + +03| | + +-------------------------------------------------------------+ +04| META_IPv4_ADDRESS | + +-------------------------------------------------------------+ +05| DEFAULT_IPv4_ADDRESS | + +-------------------------------------------------------------+ +06| META_UDP_PORT | DEFAULT_UDP_PORT | + +-------------------------------------------------------------+ +07| UNUSED | EXTRA_FLAGS |Q| + +-------------------------------------------------------------+ +08| LEASE_DURATION | + + + +09| | + +-------------------------------------------------------------+ +10| LEASE_DEADLINE | + + + +11| | + +-------------------------------------------------------------+ +12| | + + SPDP_SEQ_NR + +13| | + +-------------------------------------------------------------+ +14| | + + PUBLICATION_SEQ_NR + +15| | + +-------------------------------------------------------------+ +16| | + + SUBSCRIPTION_SEQ_NR + +17| | + +-------------------------------------------------------------+ +18| | + + MESSAGE_SEQ_NR + +19| | + +-------------------------------------------------------------+ ENDPOINT DATA ============= - 32..............24..............16..............8...............0 - +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - +---------------------------------------------------------------+ -01| ENTITYID | - +---------------------------------------------------------------+ -02| | - + + -03| GUIDPREFIX | - + + -04| | - +---------------------------------------------------------------+ -05| | - ~ ENDPOINT_BITMASK ~ -**| | - +---------------------------------------------------------------+ + 31............24..............16..............8...............0 + +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + +-------------------------------------------------------------+ +01| ENTITYID | + +-------------------------------------------------------------+ +02| | + + + +03| GUIDPREFIX | + + + +04| | + +-------------------------------------------------------------+ +05| | + ~ ENDPOINT_BITMASK ~ +**| | + +-------------------------------------------------------------+ ENDPOINT MATCH FRAME ==================== - 32..............24..............16..............8...............0 - +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - +---------------------------------------------------------------+ -01| OPCODE | - +---------------------------------------------------------------+ -02| | - + + -03| GUIDPREFIX | - + + -04| | - +---------------------------------------------------------------+ -05| ENTITYID | - +---------------------------------------------------------------+ -06| IPv4_ADDRESS | - +---------------------------------------------------------------+ -07| UDP_PORT | EXTRA_FLAGS |Q| - +---------------------------------------------------------------+ + 31............24..............16..............8...............0 + +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + +-------------------------------------------------------------+ +01| OPCODE | + +-------------------------------------------------------------+ +02| | + + + +03| GUIDPREFIX | + + + +04| | + +-------------------------------------------------------------+ +05| ENTITYID | + +-------------------------------------------------------------+ +06| IPv4_ADDRESS | + +-------------------------------------------------------------+ +07| UDP_PORT | EXTRA_FLAGS |Q| + +-------------------------------------------------------------+ ENDPOINT UNMATCH FRAME ====================== - 32..............24..............16..............8...............0 - +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - +---------------------------------------------------------------+ -01| OPCODE | - +---------------------------------------------------------------+ -02| | - + + -03| GUIDPREFIX | - + + -04| | - +---------------------------------------------------------------+ -05| ENTITYID | - +---------------------------------------------------------------+ + 31............24..............16..............8...............0 + +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + +-------------------------------------------------------------+ +01| OPCODE | + +-------------------------------------------------------------+ +02| | + + + +03| GUIDPREFIX | + + + +04| | + +-------------------------------------------------------------+ +05| ENTITYID | + +-------------------------------------------------------------+ LOCAL ENDPOINT BUFFER ===================== -32..............24..............16..............8...............0 -+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ -+---------------------------------------------------------------+ -| | -+ + -| GUIDPREFIX | -+ + -| | -+---------------------------------------------------------------+ -| ENTITYID | -+---------------------------------------------------------------+ -| IPv4_ADDRESS | -+---------------------------------------------------------------+ -| UDP_PORT | EXTRA_FLAGS | -+---------------------------------------------------------------+ -| LIFESPAN | -+ + -| (READER_ONLY) | -+---------------------------------------------------------------+ +31............24..............16..............8...............0 ++-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ ++-------------------------------------------------------------+ +| | ++ + +| GUIDPREFIX | ++ + +| | ++-------------------------------------------------------------+ +| ENTITYID | ++-------------------------------------------------------------+ +| IPv4_ADDRESS | ++-------------------------------------------------------------+ +| UDP_PORT | EXTRA_FLAGS | ++-------------------------------------------------------------+ +| LIFESPAN | ++ + +| (READER_ONLY) | ++-------------------------------------------------------------+ +OUTPUT DATA +=========== + 31............24..............16..............8...............0 + +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ +01| PACKET_LENGTH | + +-------------------------------------------------------------+ +02| SRC_IPv4_ADDRESS | + +-------------------------------------------------------------+ +03| DEST_IPv4_ADDRESS | + +-------------------------------------------------------------+ +04| SRC_UDP_PORT | DEST_UDP_PORT | + +-------------------------------------------------------------+ +05| | + ~ RTPS_MESSAGE ~ + | | + +-------------------------------------------------------------+ TOPIC KEYS diff --git a/src/math_pkg.vhd b/src/math_pkg.vhd index 9a749e3..434a8b7 100644 --- a/src/math_pkg.vhd +++ b/src/math_pkg.vhd @@ -61,7 +61,7 @@ package body math_pkg is begin ret := divident / divisor; if (divident mod divisor /= 0) then - ret := ret + divisor; + ret := ret + 1; end if; return ret; end function; diff --git a/src/rtps_builtin_endpoint.vhd b/src/rtps_builtin_endpoint.vhd index 8293e3c..4bafc11 100644 --- a/src/rtps_builtin_endpoint.vhd +++ b/src/rtps_builtin_endpoint.vhd @@ -20,6 +20,9 @@ entity rtps_builtin_endpoint is endpoint_output : out USER_ENDPOINT_OUTPUT; endpoint_full : in std_logic_vector(0 to MAX_ENDPOINTS-1); endpoint_wr : out std_logic_vector(0 to MAX_ENDPOINTS-1); + rtps_output : out std_logic_vector(31 downto 0); + rtps_wr : out std_logic; + rtps_full : in std_logic; ); end entity; @@ -86,7 +89,7 @@ architecture arch of rtps_builtin_endpoint is signal endpoint_unmatch, endpoint_unmatch_next : std_logic_vector(0 to MAX_ENDPOINTS-1) := (others => '0'); signal endpoint_mask_array, endpoint_mask_array_next : ENDPOINT_BITMASK_ARRAY_TYPE := (others => (others => '0')); signal participant_match, participant_match_next : std_logic := '0'; - signal is_requested, is_requested_next : std_logic := '0'; + signal is_subscriber, is_subscriber_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')); @@ -110,8 +113,11 @@ architecture arch of rtps_builtin_endpoint is signal mem_cnt, mem_cnt_next : integer range 0 to 3 := 0; -- NOTE: Participant Index limited to 32 bits signal participant_index, participant_index_next : std_logic_vector(31 downto 0) := (others => '0'); - signal seq_nr, seq_nr_next : std_logic_vector(SEQUENCE_NR_WIDTH-1 downto 0) := (others => '0'); - signal mem_seq_nr, mem_seq_nr_next : std_logic_vector(SEQUENCE_NR_WIDTH-1 downto 0) := (others => '0'); + signal seq_nr, seq_nr_next : unsigned(SEQUENCE_NR_WIDTH-1 downto 0) := (others => '0'); + signal mem_seq_nr, mem_seq_nr_next : unsigned(SEQUENCE_NR_WIDTH-1 downto 0) := (others => '0'); + signal next_seq_nr, next_seq_nr_next : unsigned(SEQUENCE_NR_WIDTH-1 downto 0) := (others => '0'); + signal first_seq_nr, first_seq_nr_next : unsigned(SEQUENCE_NR_WIDTH-1 downto 0) := (others => '0'); + signal last_seq_nr, last_seq_nr_next : unsigned(SEQUENCE_NR_WIDTH-1 downto 0) := (others => '0'); signal is_orphan_search, is_orphan_search_next : std_logic := '0'; signal orphan_entityid, orphan_entityid_next : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0'); signal output_sig : std_logic_vector(31 downto 0) := (others => '0'); @@ -139,6 +145,7 @@ architecture arch of rtps_builtin_endpoint is alias qos_flag_next : std_logic is flags_next(1); alias data_flag : std_logic is flags(2); alias key_flag : std_logic is flags(3); + alias final_flag : std_logic is flags(1); alias payload_flag : std_logic is header_flags(4); alias must_undersand : std_logic is parameter_id(14); @@ -267,7 +274,7 @@ begin endpoint_match_next <= endpoint_match; endpoint_unmatch_next <= endpoint_unmatch; participant_match_next <= participant_match_next; - is_requested_next <= is_requested; + is_subscriber_next <= is_subscriber; lease_duration_next <= lease_duration; lifespan_duration_next <= lifespan_duration; addr_latch_1_next <= addr_latch_1; @@ -284,6 +291,8 @@ begin output_sig <= (others => '0'); wr_sig <= '0'; stale_check_next <= stale_check; + first_seq_nr_next <= first_seq_nr; + last_seq_nr_next <= last_seq_nr; -- Last Word Latch Setter if (last_word_in = '1') then @@ -389,6 +398,8 @@ begin -- Packet Contains Participant Data message_type_next <= PDP; stage_next <= LATCH_SEQ_NR; + -- Initialise counter + cnt_next <= 1; -- Reset Participant Match participant_match_next <= '1'; end if; @@ -406,12 +417,16 @@ begin case (opcode) is when SID_HEARTBEAT => message_type_next <= EDP; - stage_next <= TODO; -- HEARTBEAT Processing + stage_next <= PROCESS_HEARTBEAT; -- HEARTBEAT Processing + -- Initialise counter + cnt_next <= 1; when SID_DATA => message_type_next <= EDP; stage_next <= LATCH_SEQ_NR; -- DATA Processing + -- Initialise counter + cnt_next <= 1; -- QoS is publisher-offered - is_requested_next <= '0'; + is_subscriber_next <= '0'; -- Reset Endpoint Mask (ALL READERS) endpoint_mask_next <= (others => '0'); endpoint_mask_next(MAX_ENDPOINTS-1 downto MAX_ENDPOINTS-NUM_READERS) <= (others => '1'); @@ -431,12 +446,16 @@ begin case (opcode) is when SID_HEARTBEAT => message_type_next <= EDP; - stage_next <= TODO; -- HEARTBEAT Processing + stage_next <= PROCESS_HEARTBEAT; -- HEARTBEAT Processing + -- Initialise counter + cnt_next <= 1; when SID_DATA => message_type_next <= EDP; stage_next <= LATCH_SEQ_NR; -- DATA Processing + -- Initialise counter + cnt_next <= 1; -- QoS is subscriber-requested - is_requested_next <= '1'; + is_subscriber_next <= '1'; -- Reset Endpoint Mask (ALL WRITERS) endpoint_mask_next <= (others => '0'); endpoint_mask_next(NUM_WRITERS-1 downto 0) <= (others => '1'); @@ -453,10 +472,14 @@ begin case (opcode) is when SID_HEARTBEAT => message_type_next <= MESSAGE; - stage_next <= TODO; -- HEARTBEAT Processing + stage_next <= PROCESS_HEARTBEAT; -- HEARTBEAT Processing + -- Initialise counter + cnt_next <= 1; when SID_DATA => message_type_next <= MESSAGE; stage_next <= LATCH_SEQ_NR; -- DATA Processing + -- Initialise counter + cnt_next <= 1; end case; when others => null; @@ -474,6 +497,8 @@ begin -- Packet Contains Participant Data message_type_next <= PDP; stage_next <= LATCH_SEQ_NR; + -- Initialise counter + cnt_next <= 1; -- Reset Participant Match participant_match_next <= '1'; @@ -492,12 +517,16 @@ begin case (opcode) is when SID_HEARTBEAT => message_type_next <= EDP; - stage_next <= TODO; -- HEARTBEAT Processing + stage_next <= PROCESS_HEARTBEAT; -- HEARTBEAT Processing + -- Initialise counter + cnt_next <= 1; when SID_DATA => message_type_next <= EDP; stage_next <= LATCH_SEQ_NR; -- DATA Processing + -- Initialise counter + cnt_next <= 1; -- QoS is publisher-offered - is_requested_next <= '0'; + is_subscriber_next <= '0'; -- Reset Endpoint Mask (ALL READERS) endpoint_mask_next <= (others => '0'); endpoint_mask_next(MAX_ENDPOINTS-1 downto MAX_ENDPOINTS-NUM_READERS) <= (others => '1'); @@ -517,12 +546,16 @@ begin case (opcode) is when SID_HEARTBEAT => message_type_next <= EDP; - stage_next <= TODO; -- HEARTBEAT Processing + stage_next <= PROCESS_HEARTBEAT; -- HEARTBEAT Processing + -- Initialise counter + cnt_next <= 1; when SID_DATA => message_type_next <= EDP; stage_next <= LATCH_SEQ_NR; -- DATA Processing + -- Initialise counter + cnt_next <= 1; -- QoS is subscriber-requested - is_requested_next <= '1'; + is_subscriber_next <= '1'; -- Reset Endpoint Mask (ALL WRITERS) endpoint_mask_next <= (others => '0'); endpoint_mask_next(NUM_WRITERS-1 downto 0) <= (others => '1'); @@ -539,10 +572,14 @@ begin case (opcode) is when SID_HEARTBEAT => message_type_next <= MESSAGE; - stage_next <= TODO; -- HEARTBEAT Processing + stage_next <= PROCESS_HEARTBEAT; -- HEARTBEAT Processing + -- Initialise counter + cnt_next <= 1; when SID_DATA => message_type_next <= MESSAGE; stage_next <= LATCH_SEQ_NR; -- DATA Processing + -- Initialise counter + cnt_next <= 1; end case; when others => null; @@ -550,12 +587,14 @@ begin when LATCH_SEQ_NR => if (empty = '0') then rd_sig <= '1'; + --Increment Counter + cnt_next <= cnt + 1; -- Latch Sequence Number case (cnt) is when 1 => - seq_nr_next(63 downto 32) <= data_in; + seq_nr_next(63 downto 32) <= unsigned(data_in); when 2 => - seq_nr_next(31 downto 0) <= data_in; + seq_nr_next(31 downto 0) <= unsigned(data_in); stage_next <= PROCESS_DATA; when others => null; @@ -828,6 +867,7 @@ begin compare_length_next <= (others => '0'); end if; when COMPARE_STRING => + -- TODO: FIX (Type Name not handled) if (empty = '0') then rd_sig <= '1'; -- Increment compare position counter @@ -857,12 +897,12 @@ begin -- Check QoS Compatibility for i in 0 to ENDPOINT_TOPIC'length-1 loop - -- data-in is Requested - if (is_requested = '1') then + -- data-in is Subscriber-Requested + if (is_subscriber = '1') then if (data_in_swapped > ENDPOINT_DURABILITY(i)) then endpoint_mask_next(i) <= '0'; end if; - -- data-in is Offered + -- data-in is Publisher-Offered else if (data_in_swapped < ENDPOINT_DURABILITY(i)) then endpoint_mask_next(i) <= '0'; @@ -878,12 +918,12 @@ begin -- Check QoS Compatibility for i in 0 to ENDPOINT_TOPIC'length-1 loop - -- data-in is Requested - if (is_requested = '1') then + -- data-in is Subscriber-Requested + if (is_subscriber = '1') then if (data_in_swapped < ENDPOINT_DEADLINE(i)(1)) then endpoint_mask_next(i) <= '0'; end if; - -- data-in is Offered + -- data-in is Publisher-Offered else if (data_in_swapped > ENDPOINT_DEADLINE(i)(1)) then endpoint_mask_next(i) <= '0'; @@ -899,12 +939,12 @@ begin -- Check QoS Compatibility for i in 0 to ENDPOINT_TOPIC'length-1 loop - -- data-in is Requested - if (is_requested = '1') then + -- data-in is Subscriber-Requested + if (is_subscriber = '1') then if (data_in_swapped < ENDPOINT_DEADLINE(i)(0)) then endpoint_mask_next(i) <= '0'; end if; - -- data-in is Offered + -- data-in is Publisher-Offered else if (data_in_swapped > ENDPOINT_DEADLINE(i)(0)) then endpoint_mask_next(i) <= '0'; @@ -920,12 +960,12 @@ begin -- Check QoS Compatibility for i in 0 to ENDPOINT_TOPIC'length-1 loop - -- data-in is Requested - if (is_requested = '1') then + -- data-in is Subscriber-Requested + if (is_subscriber = '1') then if (data_in_swapped > ENDPOINT_LIVELINESS(i)) then endpoint_mask_next(i) <= '0'; end if; - -- data-in is Offered + -- data-in is Publisher-Offered else if (data_in_swapped < ENDPOINT_LIVELINESS(i)) then endpoint_mask_next(i) <= '0'; @@ -942,12 +982,12 @@ begin rd_sig <= '1'; -- Check QoS Compatibility for i in 0 to ENDPOINT_TOPIC'length-1 loop - -- data-in is Requested - if (is_requested = '1') then + -- data-in is Subscriber-Requested + if (is_subscriber = '1') then if (data_in_swapped > ENDPOINT_LEASE_DURATION(i)(cnt)) then endpoint_mask_next(i) <= '0'; end if; - -- data-in is Offered + -- data-in is Publisher-Offered else if (data_in_swapped < ENDPOINT_LIVELINESS(i)(cnt)) then endpoint_mask_next(i) <= '0'; @@ -984,12 +1024,12 @@ begin -- Check QoS Compatibility for i in 0 to ENDPOINT_TOPIC'length-1 loop - -- data-in is Requested - if (is_requested = '1') then + -- data-in is Subscriber-Requested + if (is_subscriber = '1') then if (data_in_swapped > ENDPOINT_RELIABILITY(i)) then endpoint_mask_next(i) <= '0'; end if; - -- data-in is Offered + -- data-in is Publisher-Offered else if (data_in_swapped < ENDPOINT_RELIABILITY(i)) then endpoint_mask_next(i) <= '0'; @@ -1022,12 +1062,12 @@ begin -- Check QoS Compatibility for i in 0 to ENDPOINT_TOPIC'length-1 loop - -- data-in is Requested - if (is_requested = '1') then + -- data-in is Subscriber-Requested + if (is_subscriber = '1') then if (data_in_swapped > ENDPOINT_DESTINATION_ORDER(i)) then endpoint_mask_next(i) <= '0'; end if; - -- data-in is Offered + -- data-in is Publisher-Offered else if (data_in_swapped < ENDPOINT_DESTINATION_ORDER(i)) then endpoint_mask_next(i) <= '0'; @@ -1053,12 +1093,12 @@ begin -- Check QoS Compatibility for i in 0 to ENDPOINT_TOPIC'length-1 loop - -- data-in is Requested - if (is_requested = '1') then + -- data-in is Subscriber-Requested + if (is_subscriber = '1') then if (data_in_swapped > ENDPOINT_PRESENTATION(i)) then endpoint_mask_next(i) <= '0'; end if; - -- data-in is Offered + -- data-in is Publisher-Offered else if (data_in_swapped < ENDPOINT_PRESENTATION(i)) then endpoint_mask_next(i) <= '0'; @@ -1074,8 +1114,8 @@ begin -- Check QoS Compatibility for i in 0 to ENDPOINT_TOPIC'length-1 loop - -- data-in is Requested - if (is_requested = '1') then + -- data-in is Subscriber-Requested + if (is_subscriber = '1') then if (data_in(23) = '1' and ENDPOINT_COHERENT_ACCESS(i) = '0') then endpoint_mask_next(i) <= '0'; end if; @@ -1252,8 +1292,11 @@ begin -- Ignore all other messages, since there is no corresponding participant in the buffer. stage_next <= SKIP_PACKET; end if; - -- Match in Buffer, New Sequence Number - elsif (mem_seq_nr > seq_nr) + -- Match in Buffer, Next Sequence Number + -- NOTE: We only process the Sequences in order. This may lead to more traffic being exchanged + -- (since if a Data message with a higher sequence number than the next expected will be ignored), + -- but keeps the logic simple. + elsif (next_seq_nr = seq_nr) -- Participant if (message_type = PDP) then -- Participant Unmatch @@ -1457,6 +1500,105 @@ begin guid_next(2) <= mem_guidprefix(2); stage_next <= FIND_ORPHAN_ENDPOINT; end if; + when PROCESS_HEARTBEAT => + if (empty = '0') then + rd_sig <= '1'; + --Increment Counter + cnt_next <= cnt + 1; + -- Latch Sequence Number + case (cnt) is + when 1 => + first_seq_nr_next(63 downto 32) <= data_in; + when 2 => + first_seq_nr_next(31 downto 0) <= data_in; + when 3 => + last_seq_nr_next(63 downto 32) <= data_in; + when 4 => + last_seq_nr_next(31 downto 0) <= data_in; + stage_next <= COMPARE_SEQUENCE_NUMBERS; + end case; + end if; + when COMPARE_SEQUENCE_NUMBERS => + -- Wait for Sequence Number to be fetched from buffer + if (mem_done = '1') then + -- If current sequence number obsolete (removed from source history cache), + -- reset sequence number to expect the first available and request it + if (first_seq_nr > next_seq_nr) then + -- Store expected sequence number -1, since we process only stored sequence number +1 + seq_nr_next <= first_seq_nr - to_unsigned(1, seq_nr'length); + mem_opcode <= UPDATE_PARTICIPANT; + start_mem_op <= '1'; + -- Use "last_seq_nr" as temporal store for the ACKNACK sequence + last_seq_nr_next <= first_seq_nr; + stage_next <= SEND_ACKNACK; + cnt_next <= 1; + -- If new sequence number is available or Writer expects ACKNACK, send it + elsif (last_seq_nr > mem_seq_nr or final_flag = '0') then + -- Use "last_seq_nr" as temporal store for the ACKNACK sequence + last_seq_nr_next <= next_seq_nr; + stage_next <= SEND_ACKNACK; + cnt_next <= 1; + -- Ignore + else + -- Done + stage_next <= SKIP_PACKET; + end if; + end if; + when SEND_ACKNACK => + if (rtps_full = '0') then + wr_sig <= '1'; + --Increment Counter + cnt_next <= cnt + 1; + + case (cnt) is + -- OUTPUT HEADER + -- Packet Length + when 1 => + output_sig <= TODO; + -- Src IPv4 Address + when 2 => + output_sig <= DEFAULT_IPv4_MULTICAST_ADDRESS; + -- Dest IPv4 Address + when 3 => + output_sig <= src_addr; + -- Src and Dest UDPv4 Ports + when 4 => + output_sig <= META_IPv4_UNICAST_PORT & src_port; + -- RTPS MESSAGE HEADER + when 5 => + output_sig <= PROTOCOL_RTPS; + when 6 => + output_sig <= PROTOCOLVERSION_2_4 & VENDORID; + when 7 => + output_sig <= GUIDPREFIX(0); + when 8 => + output_sig <= GUIDPREFIX(1); + when 9 => + output_sig <= GUIDPREFIX(2); + -- ACKNACK RTPS SUBMESSAGE + -- RTPS Submessage Header + when 10 => + output_sig <= SID_ACKNACK & "00000010" & TODO; + -- Reader Entity ID + when 11 => + output_sig <= src_entityid; + -- Writer Entity ID + when 12 => + output_sig <= dest_entityid; + -- Sequence Number Set (Bitmap Base 1/2) + when 13 => + output_sig <= last_seq_nr(63 downto 32); + -- Sequence Number Set (Bitmap Base 2/2) + when 14 => + output_sig <= last_seq_nr(31 downto 0); + -- Sequence Number Set (NumBits) + output_sig <= (others => '0'); + -- Count + when 15 => + output_sig <= TODO + when 16 => + end case; + end if; --############################# when SKIP_PARAMETER => -- End of Parameter @@ -1581,7 +1723,7 @@ begin mem_addr_next <= addr_res + to_unsigned(11, mem_addr'length); when EDP => -- Publisher - if (is_requested = '0') then + if (is_subscriber = '0') then mem_addr_next <= addr_res + to_unsigned(13, mem_addr'length); -- Subscriber else @@ -1681,7 +1823,7 @@ begin mem_addr_next <= mem_addr_base + to_unsigned(11, mem_addr'length); when EDP => -- Publisher - if (is_requested = '0') then + if (is_subscriber = '0') then mem_addr_next <= mem_addr_base + to_unsigned(13, mem_addr'length); -- Subscriber else @@ -1705,9 +1847,11 @@ begin -- Memory Preload null; when 1 => - mem_seq_nr_next(63 downto 32) <= mem_read_data; + mem_seq_nr_next(63 downto 32) <= unsigned(mem_read_data); when 2 => - mem_seq_nr_next(31 downto 0) <= mem_read_data; + mem_seq_nr_next(31 downto 0) <= unsigned(mem_read_data); + when 3 => + next_seq_nr_next <= mem_seq_nr + to_unsigned(1,mem_seq_nr'length); mem_stage_next <= IDLE; end case; when SEARCH_ENDPOINT => @@ -1954,10 +2098,10 @@ begin mem_write_data <= lease_deadline(1); when 12 => -- SPDP Sequence Number 1/2 - mem_write_data <= seq_nr(0); + mem_write_data <= std_logic_vector(seq_nr(63 downto 32)); when 13 => -- SPDP Sequence Number 2/2 - mem_write_data <= seq_nr(1); + mem_write_data <= std_logic_vector(seq_nr(31 downto 0)); when 14 => -- Publication Sequence Number 1/2 mem_write_data <= (others => '0'); @@ -2123,9 +2267,9 @@ begin 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); + mem_write_data <= std_logic_vector(seq_nr(63 downto 32)); when 1 => - mem_write_data <= seq_nr(31 downto 0); + mem_write_data <= std_logic_vector(seq_nr(31 downto 0)); mem_stage_next <= IDLE; end case; when others => diff --git a/src/rtps_package.vhd b/src/rtps_package.vhd index 0b5d4b5..3648b92 100644 --- a/src/rtps_package.vhd +++ b/src/rtps_package.vhd @@ -36,12 +36,15 @@ package rtps_package is constant MAC_ADDRESS : std_logic_vector(47 downto 0) := x"97917E0BA8CF"; -- Domain ID constant USER_DOMAIN_ID : integer := 1; + -- Domain TAG + constant USER_DOMAIN_TAG : string := ""; ----------------------------------------------------------------------------------------------------- -- *DO NOT MODIFY BEGIN* type ENDPOINT_WITH_KEY_TYPE is array (0 to MAX_ENDPOINTS-1) of boolean; - type ENDPOINT_TOPIC_STRING_TYPE is array (0 to MAX_ENDPOINTS-1) of string(1 to 256); - type ENDPOINT_TOPIC_TYPE is array (0 to MAX_ENDPOINTS-1) of std_logic_vector(0 to (256*8)-1); + type ENDPOINT_STRING_TYPE is array (0 to MAX_ENDPOINTS-1) of string(1 to 256); + type STRING_SLV_WORD_TYPE is array (0 to (256*8)/32) of std_logic_vector(31 downto 0); + type ENDPOINT_STRING_SLV_TYPE is array (0 to MAX_ENDPOINTS-1) of STRING_SLV_WORD_TYPE; subtype QOS_TYPE is array (0 to MAX_ENDPOINTS-1) of std_logic_vector(31 downto 0); subtype QOS_SLV_TYPE is array (0 to MAX_ENDPOINTS-1) of std_logic_vector(31 downto 0); type DURATION_TYPE is array (0 to 1) of unsigned(31 downto 0); @@ -85,8 +88,10 @@ package rtps_package is constant ENDPOINT_WITH_KEY : ENDPOINT_WITH_KEY_TYPE := (0 => FALSE); -- Array mapping Topic Strings to Endpoints -- NOTE: All strings have to be padded to 256 characters - constant ENDPOINT_TOPIC_STRING : ENDPOINT_TOPIC_STRING_TYPE := (0 => "Placeholder" & (12 to 256 => NUL)); - constant ENDPOINT_TOPIC : ENDPOINT_TOPIC_TYPE; -- Deferred to Package Body + constant ENDPOINT_TOPIC_STRING : ENDPOINT_STRING_TYPE := (0 => "Placeholder" & (12 to 256 => NUL)); + constant ENDPOINT_TOPIC : ENDPOINT_STRING_SLV_TYPE; -- Deferred to Package Body + constant ENDPOINT_TYPE_STRING : ENDPOINT_STRING_TYPE := (0 => "Placeholder" & (12 to 256 => NUL)); + constant ENDPOINT_TYPE : ENDPOINT_STRING_SLV_TYPE; -- Deferred to Package Body constant ENDPOINT_DURABILITY : QOS_TYPE := (0 => VOLATILE_DURABILITY_QOS); constant ENDPOINT_PRESENTATION : QOS_TYPE := (0 => INSTANCE_PRESENTATION_QOS); constant ENDPOINT_COHERENT_ACCESS : std_logic_vector(0 to MAX_ENDPOINTS-1) := (others => '0'); @@ -97,7 +102,7 @@ package rtps_package is -- Only relevant for Readers constant ENDPOINT_TIME_BASED_FILTER : ENDPOINT_DURATION_TYPE := (0 => (to_unsigned(0,32),to_unsigned(0,32))); --TODO: Assert constant ENDPOINT_RELIABILITY : QOS_TYPE := (0 => RELIABLE_RELIABILITY_QOS); - constant ENDPOINT_MAX_BLOCKING_TIME : ENDPOINT_DURATION_TYPE := (0 => (to_unsigned(0,32),to_unsigned(1,32))); + constant ENDPOINT_MAX_BLOCKING_TIME : ENDPOINT_DURATION_TYPE := (0 => (to_unsigned(0,32),to_unsigned(429496730,32))); --ca 100ms -- Only relevant for Writers constant ENDPOINT_LIFESPAN : ENDPOINT_DURATION_TYPE := (0 => DURATION_INFINITE); constant ENDPOINT_DESTINATION_ORDER : QOS_TYPE := (0 => BY_RECEPTION_TIMESTAMP_DESTINATIONORDER_QOS); @@ -107,6 +112,9 @@ package rtps_package is constant ENDPOINT_MAX_INSTANCES : QOS_SLV_TYPE := (0 => std_logic_vector(to_unsigned(0,32))); --TODO: Assert constant ENDPOINT_MAX_SAMP_PER_INST : QOS_SLV_TYPE := (0 => std_logic_vector(to_unsigned(0,32))); --TODO: Assert + constant DEFAULT_PARTICIPANT_LEASE_DURATION : DURATION_TYPE := (to_unsigned(100,32),to_unsigned(0,32); + constant PARTICIPANT_LEASE_DURATION : DURATION_TYPE := DEFAULT_PARTICIPANT_LEASE_DURATION; + -- NOTE: The buffer will not only store participants, but also endpoint data -- Used to determine the size of the built-inendpoint buffer constant MAX_REMOTE_PARTICIPANTS : integer := 50; @@ -229,6 +237,7 @@ package rtps_package is constant DOMAIN_ID : std_logic_vector(DOMAIN_ID_WIDTH-1 downto 0) := to_unsigned(USER_DOMAIN_ID, DOMAIN_ID'length); + constant DOMAIN_TAG : STRING_SLV_WORD_TYPE; -- Deferred to packgae body -- Since this implementation runs on the same network stack and the RTPS Endpoints (Readers & Writers) -- can be differentiated based on their Entity ID, it makes no sense to have multiple IP Addresses @@ -251,15 +260,11 @@ package rtps_package is -- participant_id=0 constant USER_IPv4_UNICAST_PORT : std_logic_vector(UDP_PORT_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(PORT_CONFIG_PB + PORT_CONFIG_D3 + PORT_CONFIG_DG*USER_DOMAIN_ID, UDP_PORT_WIDTH)); - -- First two bytes have to be Vendor ID (see DDSI-RTPS 2.3 Section 9.3.1.5) - -- Next we insert the MAC address for uniqueness - -- Next we insert the Domain ID - type GUIDPREFIX_ARRAY_TYPE is array (0 to GUIDPREFIX_WIDTH/32-1) of std_logic_vector(31 downto 0); - type GUID_ARRAY_TYPE is array (0 to (GUIDPREFIX_WIDTH+ENTITYID_WIDTH)/32-1) of std_logic_vector(31 downto 0); - - constant GUIDPREFIX : std_logic_vector(GUIDPREFIX_WIDTH-1 downto 0) := (GUIDPREFIX_WIDTH-1 downto GUIDPREFIX_WIDTH-VENDORID_WIDTH => VENDORID, GUIDPREFIX_WIDTH-VENDORID_WIDTH-1 downto GUIDPREFIX_WIDTH-VENDORID_WIDTH-48 => MAC_ADDRESS, DOMAIN_ID_WIDTH-1 downto 0 => DOMAIN_ID); - constant GUIDPREFIX_UNKNOWN : std_logic_vector(GUIDPREFIX_WIDTH-1 downto 0) := (others => '0'); - constant GUIDPREFIX_UNKNOWN_ARRAY : GUIDPREFIX_ARRAY_TYPE := (others => (others => '0')); + type GUIDPREFIX_TYPE is array (0 to GUIDPREFIX_WIDTH/32-1) of std_logic_vector(31 downto 0); + type GUID_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 : GUIDPREFIX_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); @@ -329,9 +334,15 @@ package rtps_package is type USER_ENDPOINT_OUTPUT is array (0 to MAX_ENDPOINTS-1) of std_logic_vector(31 downto 0); type ENDPOINT_BITMASK_ARRAY_TYPE is array (0 to ENDPOINT_BITMASK_SIZE-1) of std_logic_vector(31 downto 0); - -- TODO - type BUILTIN_ENDPOINT_DATA_TYPE is array (range <>) of std_logic_vector(31 downto 0); - constant BUILTIN_ENDPOINT_DATA : BUILTIN_ENDPOINT_DATA_TYPE; --Deferred to package body + type WORD_ARRAY_TYPE is array (range <>) of std_logic_vector(31 downto 0); + type OUTPUT_DATA_TYPE is record + data : WORD_ARRAY_TYPE; + length : integer; + end record; + + constant READER_ENDPOINT_DATA : OUTPUT_DATA_TYPE; --Deferred to package body + constant WRITER_ENDPOINT_DATA : OUTPUT_DATA_TYPE; --Deferred to package body + constant PARTICIPANT_DATA : OUTPUT_DATA_TYPE; --Deferred to package body end package; @@ -366,18 +377,559 @@ package body rtps_package is constant ENTITYID : ENTITYID_TYPE := gen_entyid; - function convert_string (string_array : ENDPOINT_TOPIC_STRING_TYPE) return ENDPOINT_TOPIC_TYPE is - variable ret : ENDPOINT_TOPIC_TYPE + + function gen_guidprefix return GUIDPREFIX_TYPE is + variable tmp : std_logic_vector(GUIDPREFIX_WIDTH-1 downto 0) := (others => '0'); + variable ret : GUIDPREFIX_TYPE := (others => (others => '0')); begin + tmp := (others => '0'); ret := (others => (others => '0')); + -- First two bytes have to be Vendor ID (see DDSI-RTPS 2.3 Section 9.3.1.5) + tmp(GUIDPREFIX_WIDTH-1 downto GUIDPREFIX_WIDTH-VENDORID_WIDTH) <= VENDORID; + -- Next we insert the MAC address for uniqueness + tmp(GUIDPREFIX_WIDTH-VENDORID_WIDTH-1 downto GUIDPREFIX_WIDTH-VENDORID_WIDTH-48) <= MAC_ADDRESS; + -- Next we insert the Domain ID + tmp(DOMAIN_ID_WIDTH-1 downto 0) <= DOMAIN_ID; + + -- Convert to return type for i in 0 to ret'length-1 loop - for j in 1 to ret'length(1) loop - ret(i)((256-j)*8 to ((257-j)*8)-1) := std_logic_vector(to_unsigned(character'POS(string_array(i)(j)), 8)); + ret(i) := tmp(GUIDPREFIX_WIDTH-(i*ret'length(1))-1 downto GUIDPREFIX_WIDTH-(i*ret'length(1))-ret'length(1)); + end loop; + return ret; + end function; + + constant GUIDPREFIX : GUIDPREFIX_TYPE := gen_guidprefix; + + function convert_string (str : string) return STRING_SLV_WORD_TYPE is + variable ret : STRING_SLV_WORD_TYPE := (others => (others => '0')); + begin + ret := (others => others => '0')); + for i in 0 to ret'length-1 loop + ret(i) := std_logic_vector(to_unsigned(character'POS(str(i)), 8)) & std_logic_vector(to_unsigned(character'POS(str(i+1)), 8)) & std_logic_vector(to_unsigned(character'POS(str(i+2)), 8)) & std_logic_vector(to_unsigned(character'POS(str(i+3)), 8)); + end loop; + return ret; + end function; + + function convert_endpoint_string (string_array : ENDPOINT_STRING_TYPE) return ENDPOINT_STRING_SLV_TYPE is + variable ret : ENDPOINT_STRING_SLV_TYPE := (others => (others => (others => '0'))); + begin + ret := (others => (others => (others => '0'))); + for i in 0 to ret'length-1 loop + ret(i) := convert_string(string_array(i)); + end loop; + return ret; + end function; + + constant ENDPOINT_TOPIC : ENDPOINT_STRING_SLV_TYPE := convert_endpoint_string(ENDPOINT_TOPIC_STRING); + constant ENDPOINT_TYPE : ENDPOINT_STRING_SLV_TYPE := convert_endpoint_string(ENDPOINT_TYPE_STRING); + constant DOMAIN_TAG : STRING_SLV_WORD_TYPE := convert_string(USER_DOMAIN_TAG); + + function string_len (str : STRING_SLV_WORD_TYPE) return integer is + ret : integer := 0; + begin + ret := 0; + for i in 0 to str'length loop + for j in 0 to str'length(1)-1 loop + -- Count Bytes + ret := ret + 1; + -- Exit on first NULL byte (NULL Byte included in count) + if (str(i)(str'length(1)-j*8-1 downto str'length(1)-j*8-8) = (8 downto 0 => '0')) then + exit; + end if; end loop; end loop; return ret; end function; - constant ENDPOINT_TOPIC : ENDPOINT_TOPIC_TYPE := convert_string(ENDPOINT_TOPIC_STRING); + function gen_reader_endpoint_data return OUTPUT_DATA_TYPE is + -- Limit DATA to MAX UDPv4 Payload Size (65,507 Bytes) + variable ret : OUTPUT_DATA_TYPE(data(0 to 16376)) := (data => (others => (others => '0')), length => 0); + variable ind : integer := 0; + variable len : integer := 0; + variable tmp : integer := 0; + begin + ret.data := (others => (others => '0')); + ret.length := 0; + len := 0; + ind := 0; + ind2 := 0; + -- RTPS HEADER + ret.data(0) := PROTOCOL_RTPS; + ret.data(1) := PROTOCOLVERSION_2_4 & VENDORID; + ret.data(2) := GUIDPREFIX(0); + ret.data(3) := GUIDPREFIX(1); + ret.data(4) := GUIDPREFIX(2); + ind := 5; + -- RTPS Submessages + -- One DATA Submessage for each Reader + for i in 0 to NUM_READERS-1 loop + -- RTPS Submessage Header + ret.data(ind) := SID_DATA & "00000100" & x"0000"; + -- DATA Header (extraFlags, octetsToInlineQoS) + len := len + 1; + ret.data(ind+len) := x"0000" & std_logic_vector(to_unsigned(16, 16)); + -- DATA Header (Reader Entity ID) + len := len + 1; + ret.data(ind+len) := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR; + -- DATA Header (Writer Entity ID) + len := len + 1; + ret.data(ind+len) := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER; + -- DATA Header (Sequence Number) + len := len + 1; + ret.data(ind+len) := (others => '0'); + -- DATA Header (Sequence Number) + len := len + 1; + ret.data(ind+len) := std_logic_vector(to_unsigned(i+1, ret.data'length(1))); + -- Serialized Payload Header + len := len + 1; + ret.data(ind+len) := PL_CDR_BE & x"0000"; + -- Serialized Payload BEGIN + -- GUID + len := len + 1; + ret.data(ind+len):= PID_ENDPOINT_GUID & std_logic_vector(to_unsigned(16, 16)); + len := len + 1; + ret.data(ind+len) := GUIDPREFIX(0); + len := len + 1; + ret.data(ind+len) := GUIDPREFIX(1); + len := len + 1; + ret.data(ind+len) := GUIDPREFIX(2); + len := len + 1; + ret.data(ind+len) := ENTITYID(i); + -- EXPECTS INLINE QOS (Only relevant for Reader endpoints) + if (i < NUM_READERS) then + len := len + 1; + ret.data(ind+len) := PID_EXPECTS_INLINE_QOS & std_logic_vector(to_unsigned(4, 16)); + len := len + 1; + ret.data(ind+len) := (24 => '1', others => '0'); + end if; + -- TOPIC NAME + tmp := string_len(ENDPOINT_TOPIC(i)); + len := len + 1; + ret.data(ind+len) := PID_TOPIC_NAME & std_logic_vector(to_unsigned((round_div(tmp,4)+1)*4, 16)); + len := len + 1; + ret.data(ind+len) := std_logic_vector(to_unsigned(tmp, 32)); + for j in 0 to tmp-1 loop + len := len + 1; + ret.data(ind+len) := ENDPOINT_TOPIC(i)(j); + end loop; + -- TYPE NAME + tmp := string_len(ENDPOINT_TYPE(i)); + len := len + 1; + ret.data(ind+len) := PID_TYPE_NAME & std_logic_vector(to_unsigned((round_div(tmp,4)+1)*4, 16)); + len := len + 1; + ret.data(ind+len) := std_logic_vector(to_unsigned(tmp, 32)); + for j in 0 to tmp-1 loop + len := len + 1; + ret.data(ind+len) := ENDPOINT_TYPE(i)(j); + end loop; + -- DURABILITY + if (ENDPOINT_DURABILITY(i) /= VOLATILE_DURABILITY_QOS) then + len := len + 1; + ret.data(ind+len) := PID_DURABILITY & std_logic_vector(to_unsigned(4, 16)); + len := len + 1; + ret.data(ind+len) := ENDPOINT_DURABILITY(i); + end if; + -- DEADLINE + if (ENDPOINT_DEADLINE(i) /= DURATION_INFINITE) then + len := len + 1; + ret.data(ind+len) := PID_DEADLINE & std_logic_vector(to_unsigned(8, 16)); + len := len + 1; + ret.data(ind+len) := ENDPOINT_DEADLINE(i)(0); + len := len + 1; + ret.data(ind+len) := ENDPOINT_DEADLINE(i)(1); + end if; + -- LIVELINESS + if (ENDPOINT_LIVELINESS(i) /= AUTOMATIC_LIVELINESS_QOS or ENDPOINT_LEASE_DURATION(i) /= DURATION_INFINITE) then + len := len + 1; + ret.data(ind+len) := PID_LIVELINESS & std_logic_vector(to_unsigned(12, 16)); + len := len + 1; + ret.data(ind+len) := ENDPOINT_LIVELINESS(i); + len := len + 1; + ret.data(ind+len) := ENDPOINT_LEASE_DURATION(i)(0); + len := len + 1; + ret.data(ind+len) := ENDPOINT_LEASE_DURATION(i)(1); + end if; + -- RELIABILITY + -- TODO: Check also max_blocking_time for change (Currently ignored) + if (ENDPOINT_RELIABILITY(i) /= RELIABLE_RELIABILITY_QOS) then + len := len + 1; + ret.data(ind+len) := PID_RELIABILITY & std_logic_vector(to_unsigned(12, 16)); + len := len + 1; + ret.data(ind+len) := ENDPOINT_RELIABILITY(i); + len := len + 1; + ret.data(ind+len) := ENDPOINT_MAX_BLOCKING_TIME(i)(0); + len := len + 1; + ret.data(ind+len) := ENDPOINT_MAX_BLOCKING_TIME(i)(1); + end if; + -- DESTINATION ORDER + if (ENDPOINT_DESTINATION_ORDER(i) /= BY_RECEPTION_TIMESTAMP_DESTINATIONORDER_QOS) then + len := len + 1; + ret.data(ind+len) := PID_DESTINATION_ORDER & std_logic_vector(to_unsigned(4, 16)); + len := len + 1; + ret.data(ind+len) := ENDPOINT_DESTINATION_ORDER(i); + end if; + -- PRESENTATION + if (ENDPOINT_PRESENTATION(i) /= INSTANCE_PRESENTATION_QOS) then + len := len + 1; + ret.data(ind+len) := PID_PRESENTATION & std_logic_vector(to_unsigned(8, 16)); + len := len + 1; + ret.data(ind+len) := ENDPOINT_PRESENTATION(i); + len := len + 1; + ret.data(ind+len) := (24 => ENDPOINT_COHERENT_ACCESS(i), 16 => ENDPOINT_ORDERED_ACCESS(i), others => '0'); + end if; + -- SENTINEL + len := len + 1; + ret.data(ind+len) := PID_SENTINEL & std_logic_vector(to_unsigned(0, 16)); + -- Fix Submessage Length + ret.data(ind)(15 downto 0) := std_logic_vector(to_unsigned(len*4, 16)); + -- Reset Pointers + ind := ind + len + 1; + len := 0; + end loop; + -- Store Total Length + ret.length := ind; + return ret; + end function; + + constant READER_ENDPOINT_DATA : OUTPUT_DATA_TYPE := gen_reader_endpoint_data; + + function gen_writer_endpoint_data return OUTPUT_DATA_TYPE is + -- Limit DATA to MAX UDPv4 Payload Size (65,507 Bytes) + variable ret : OUTPUT_DATA_TYPE(data(0 to 16376)) := (data => (others => (others => '0')), length => 0); + variable ind : integer := 0; + variable len : integer := 0; + variable tmp : integer := 0; + begin + ret.data := (others => (others => '0')); + ret.length := 0; + len := 0; + ind := 0; + ind2 := 0; + -- RTPS HEADER + ret.data(0) := PROTOCOL_RTPS; + ret.data(1) := PROTOCOLVERSION_2_4 & VENDORID; + ret.data(2) := GUIDPREFIX(0); + ret.data(3) := GUIDPREFIX(1); + ret.data(4) := GUIDPREFIX(2); + ind := 5; + -- RTPS Submessages + -- One DATA Submessage for each Endpoint + for i in NUM_READERS to MAX_ENDPOINTS-1 loop + -- RTPS Submessage Header + ret.data(ind) := SID_DATA & "00000100" & x"0000"; + -- DATA Header (extraFlags, octetsToInlineQoS) + len := len + 1; + ret.data(ind+len) := x"0000" & std_logic_vector(to_unsigned(16, 16)); + -- DATA Header (Reader Entity ID) + len := len + 1; + ret.data(ind+len) := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR; + -- DATA Header (Writer Entity ID) + len := len + 1; + ret.data(ind+len) := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER; + -- DATA Header (Sequence Number) + len := len + 1; + ret.data(ind+len) := (others => '0'); + -- DATA Header (Sequence Number) + len := len + 1; + ret.data(ind+len) := std_logic_vector(to_unsigned(i-NUM_READERS+1, ret.data'length(1))); + -- Serialized Payload Header + len := len + 1; + ret.data(ind+len) := PL_CDR_BE & x"0000"; + -- Serialized Payload BEGIN + -- GUID + len := len + 1; + ret.data(ind+len):= PID_ENDPOINT_GUID & std_logic_vector(to_unsigned(16, 16)); + len := len + 1; + ret.data(ind+len) := GUIDPREFIX(0); + len := len + 1; + ret.data(ind+len) := GUIDPREFIX(1); + len := len + 1; + ret.data(ind+len) := GUIDPREFIX(2); + len := len + 1; + ret.data(ind+len) := ENTITYID(i); + -- EXPECTS INLINE QOS (Only relevant for Reader endpoints) + if (i < NUM_READERS) then + len := len + 1; + ret.data(ind+len) := PID_EXPECTS_INLINE_QOS & std_logic_vector(to_unsigned(4, 16)); + len := len + 1; + ret.data(ind+len) := (24 => '1', others => '0'); + end if; + -- TOPIC NAME + tmp := string_len(ENDPOINT_TOPIC(i)); + len := len + 1; + ret.data(ind+len) := PID_TOPIC_NAME & std_logic_vector(to_unsigned((round_div(tmp,4)+1)*4, 16)); + len := len + 1; + ret.data(ind+len) := std_logic_vector(to_unsigned(tmp, 32)); + for j in 0 to tmp-1 loop + len := len + 1; + ret.data(ind+len) := ENDPOINT_TOPIC(i)(j); + end loop; + -- TYPE NAME + tmp := string_len(ENDPOINT_TYPE(i)); + len := len + 1; + ret.data(ind+len) := PID_TYPE_NAME & std_logic_vector(to_unsigned((round_div(tmp,4)+1)*4, 16)); + len := len + 1; + ret.data(ind+len) := std_logic_vector(to_unsigned(tmp, 32)); + for j in 0 to tmp-1 loop + len := len + 1; + ret.data(ind+len) := ENDPOINT_TYPE(i)(j); + end loop; + -- DURABILITY + if (ENDPOINT_DURABILITY(i) /= VOLATILE_DURABILITY_QOS) then + len := len + 1; + ret.data(ind+len) := PID_DURABILITY & std_logic_vector(to_unsigned(4, 16)); + len := len + 1; + ret.data(ind+len) := ENDPOINT_DURABILITY(i); + end if; + -- DEADLINE + if (ENDPOINT_DEADLINE(i) /= DURATION_INFINITE) then + len := len + 1; + ret.data(ind+len) := PID_DEADLINE & std_logic_vector(to_unsigned(8, 16)); + len := len + 1; + ret.data(ind+len) := ENDPOINT_DEADLINE(i)(0); + len := len + 1; + ret.data(ind+len) := ENDPOINT_DEADLINE(i)(1); + end if; + -- LIVELINESS + if (ENDPOINT_LIVELINESS(i) /= AUTOMATIC_LIVELINESS_QOS or ENDPOINT_LEASE_DURATION(i) /= DURATION_INFINITE) then + len := len + 1; + ret.data(ind+len) := PID_LIVELINESS & std_logic_vector(to_unsigned(12, 16)); + len := len + 1; + ret.data(ind+len) := ENDPOINT_LIVELINESS(i); + len := len + 1; + ret.data(ind+len) := ENDPOINT_LEASE_DURATION(i)(0); + len := len + 1; + ret.data(ind+len) := ENDPOINT_LEASE_DURATION(i)(1); + end if; + -- RELIABILITY + -- TODO: Check also max_blocking_time for change (Currently ignored) + if (ENDPOINT_RELIABILITY(i) /= RELIABLE_RELIABILITY_QOS) then + len := len + 1; + ret.data(ind+len) := PID_RELIABILITY & std_logic_vector(to_unsigned(12, 16)); + len := len + 1; + ret.data(ind+len) := ENDPOINT_RELIABILITY(i); + len := len + 1; + ret.data(ind+len) := ENDPOINT_MAX_BLOCKING_TIME(i)(0); + len := len + 1; + ret.data(ind+len) := ENDPOINT_MAX_BLOCKING_TIME(i)(1); + end if; + -- LIFESPAN + if (ENDPOINT_LIFESPAN(i) /= DURATION_INFINITE) then + len := len + 1; + ret.data(ind+len) := PID_LIFESPAN & std_logic_vector(to_unsigned(8, 16)); + len := len + 1; + ret.data(ind+len) := ENDPOINT_LIFESPAN(i)(0); + len := len + 1; + ret.data(ind+len) := ENDPOINT_LIFESPAN(i)(1); + end if; + -- DESTINATION ORDER + if (ENDPOINT_DESTINATION_ORDER(i) /= BY_RECEPTION_TIMESTAMP_DESTINATIONORDER_QOS) then + len := len + 1; + ret.data(ind+len) := PID_DESTINATION_ORDER & std_logic_vector(to_unsigned(4, 16)); + len := len + 1; + ret.data(ind+len) := ENDPOINT_DESTINATION_ORDER(i); + end if; + -- PRESENTATION + if (ENDPOINT_PRESENTATION(i) /= INSTANCE_PRESENTATION_QOS) then + len := len + 1; + ret.data(ind+len) := PID_PRESENTATION & std_logic_vector(to_unsigned(8, 16)); + len := len + 1; + ret.data(ind+len) := ENDPOINT_PRESENTATION(i); + len := len + 1; + ret.data(ind+len) := (24 => ENDPOINT_COHERENT_ACCESS(i), 16 => ENDPOINT_ORDERED_ACCESS(i), others => '0'); + end if; + -- SENTINEL + len := len + 1; + ret.data(ind+len) := PID_SENTINEL & std_logic_vector(to_unsigned(0, 16)); + -- Fix Submessage Length + ret.data(ind)(15 downto 0) := std_logic_vector(to_unsigned(len*4, 16)); + -- Reset Pointers + ind := ind + len + 1; + len := 0; + end loop; + -- Store Total Length + ret.length := ind; + return ret; + end function; + + constant ENDPOINT_DATA : OUTPUT_DATA_TYPE := gen_writer_endpoint_data; + + function gen_participant_data return OUTPUT_DATA_TYPE is + -- Limit DATA to MAX UDPv4 Payload Size (65,507 Bytes) + variable ret : OUTPUT_DATA_TYPE(data(0 to 16376)) := (data => (others => (others => '0')), length => 0); + variable ind : integer := 0; + variable len : integer := 0; + variable tmp : integer := 0; + begin + ret.data := (others => (others => '0')); + ret.length := 0; + len := 0; + ind := 0; + ind2 := 0; + -- RTPS HEADER + ret.data(0) := PROTOCOL_RTPS; + ret.data(1) := PROTOCOLVERSION_2_4 & VENDORID; + ret.data(2) := GUIDPREFIX(0); + ret.data(3) := GUIDPREFIX(1); + ret.data(4) := GUIDPREFIX(2); + ind := 5; + -- RTPS DATA SUBMESSAGE + -- RTPS Submessage Header + ret.data(ind) := SID_DATA & "00000100" & x"0000"; + -- DATA Header (extraFlags, octetsToInlineQoS) + len := len + 1; + ret.data(ind+len) := x"0000" & std_logic_vector(to_unsigned(16, 16)); + -- DATA Header (Reader Entity ID) + len := len + 1; + ret.data(ind+len) := ENTITYID_SPDP_BUILTIN_PARTICIPANT_DETECTOR; + -- DATA Header (Writer Entity ID) + len := len + 1; + ret.data(ind+len) := ENTITYID_SPDP_BUILTIN_PARTICIPANT_ANNOUNCER; + -- DATA Header (Sequence Number) + len := len + 1; + ret.data(ind+len) := (others => '0'); + -- DATA Header (Sequence Number) + len := len + 1; + ret.data(ind+len) := std_logic_vector(to_unsigned(1, ret.data'length(1))); + -- Serialized Payload Header + len := len + 1; + ret.data(ind+len) := PL_CDR_BE & x"0000"; + -- Serialized Payload BEGIN + -- GUID + len := len + 1; + ret.data(ind+len):= PID_PARTICIPANT_GUID & std_logic_vector(to_unsigned(16, 16)); + len := len + 1; + ret.data(ind+len) := GUIDPREFIX(0); + len := len + 1; + ret.data(ind+len) := GUIDPREFIX(1); + len := len + 1; + ret.data(ind+len) := GUIDPREFIX(2); + len := len + 1; + ret.data(ind+len) := ENTITYID_PARTICIPANT; + -- DOMAIN ID + len := len + 1; + ret.data(ind+len):= PID_DOMAIN_ID & std_logic_vector(to_unsigned(4, 16)); + len := len + 1; + ret.data(ind+len):= DOMAIN_ID; + -- DOMAIN TAG + if (USER_DOMAIN_TAG /= "") then + tmp := string_len(DOMAIN_TAG); + len := len + 1; + ret.data(ind+len) := PID_DOMAIN_TAG & std_logic_vector(to_unsigned((round_div(tmp,4)+1)*4, 16)); + len := len + 1; + ret.data(ind+len) := std_logic_vector(to_unsigned(tmp, 32)); + for j in 0 to tmp-1 loop + len := len + 1; + ret.data(ind+len) := DOMAIN_TAG(j); + end loop; + end if; + -- PROTOCOL VERSION + len := len + 1; + ret.data(ind+len) := PID_PROTOCOL_VERSION & std_logic_vector(to_unsigned(4, 16)); + len := len + 1; + ret.data(ind+len) := (31 downto 16 => PROTOCOLVERSION_2_4, others => '0'); + -- VENDORID + len := len + 1; + ret.data(ind+len) := PID_VENDORID & std_logic_vector(to_unsigned(4, 16)); + len := len + 1; + ret.data(ind+len) := (31 downto 16 => VENDORID, others => '0'); + -- TODO: Expects inline QoS of Participant + -- METATRAFFIC UNICAST LOCATOR + len := len + 1; + ret.data(ind+len) := PID_METATRAFFIC_UNICAST_LOCATOR & std_logic_vector(to_unsigned(24, 16)); + len := len + 1; + ret.data(ind+len) := LOCATOR_KIND_UDPv4; + len := len + 1; + ret.data(ind+len) := (16 downto 0 => META_IPv4_UNICAST_PORT, others => '0'); + len := len + 1; + ret.data(ind+len) := (others => '0'); + len := len + 1; + ret.data(ind+len) := (others => '0'); + len := len + 1; + ret.data(ind+len) := (others => '0'); + len := len + 1; + ret.data(ind+len) := DEFAULT_IPv4_MULTICAST_ADDRESS; + -- METATRAFFIC MULTICAST LOCATOR + len := len + 1; + ret.data(ind+len) := PID_METATRAFFIC_MULTICAST_LOCATOR & std_logic_vector(to_unsigned(24, 16)); + len := len + 1; + ret.data(ind+len) := LOCATOR_KIND_UDPv4; + len := len + 1; + ret.data(ind+len) := (16 downto 0 => META_IPv4_MULTICAST_PORT, others => '0'); + len := len + 1; + ret.data(ind+len) := (others => '0'); + len := len + 1; + ret.data(ind+len) := (others => '0'); + len := len + 1; + ret.data(ind+len) := (others => '0'); + len := len + 1; + ret.data(ind+len) := DEFAULT_IPv4_MULTICAST_ADDRESS; + -- DEFAULT UNICAST LOCATOR + len := len + 1; + ret.data(ind+len) := PID_DEFAULT_UNICAST_LOCATOR & std_logic_vector(to_unsigned(24, 16)); + len := len + 1; + ret.data(ind+len) := LOCATOR_KIND_UDPv4; + len := len + 1; + ret.data(ind+len) := (16 downto 0 => USER_IPv4_UNICAST_PORT, others => '0'); + len := len + 1; + ret.data(ind+len) := (others => '0'); + len := len + 1; + ret.data(ind+len) := (others => '0'); + len := len + 1; + ret.data(ind+len) := (others => '0'); + len := len + 1; + ret.data(ind+len) := DEFAULT_IPv4_MULTICAST_ADDRESS; + -- DEFAULT MULTICAST LOCATOR + len := len + 1; + ret.data(ind+len) := PID_METATRAFFIC_MULTICAST_LOCATOR & std_logic_vector(to_unsigned(24, 16)); + len := len + 1; + ret.data(ind+len) := LOCATOR_KIND_UDPv4; + len := len + 1; + ret.data(ind+len) := (16 downto 0 => META_IPv4_MULTICAST_PORT, others => '0'); + len := len + 1; + ret.data(ind+len) := (others => '0'); + len := len + 1; + ret.data(ind+len) := (others => '0'); + len := len + 1; + ret.data(ind+len) := (others => '0'); + len := len + 1; + ret.data(ind+len) := DEFAULT_IPv4_MULTICAST_ADDRESS; + -- LEASE DURATION + if (PARTICIPANT_LEASE_DURATION /= DEFAULT_PARTICIPANT_LEASE_DURATION) then + len := len + 1; + ret.data(ind+len) := PID_PARTICIPANT_LEASE_DURATION & std_logic_vector(to_unsigned(8, 16)); + len := len + 1; + ret.data(ind+len) := PARTICIPANT_LEASE_DURATION(0); + len := len + 1; + ret.data(ind+len) := PARTICIPANT_LEASE_DURATION(1); + end if; + -- AVAILABLE ENDPOINTS + len := len + 1; + ret.data(ind+len) := PID_BUILTIN_ENDPOINT_SET & std_logic_vector(to_unsigned(4, 16)); + len := len + 1; + ret.data(ind+len) := (DISC_BUILTIN_ENDPOINT_PARTICIPANT_ANNOUNCER => '1', DISC_BUILTIN_ENDPOINT_PARTICIPANT_DETECTOR => '1', BUILTIN_ENDPOINT_PARTICIPANT_MESSAGE_DATA_WRITER => '1', BUILTIN_ENDPOINT_PARTICIPANT_MESSAGE_DATA_READER => '1', others => '0'); + if (NUM_READERS > 0) then + ret.data(ind+len)(DISC_BUILTIN_ENDPOINT_PUBLICATIONS_DETECTOR) := '1'; + ret.data(ind+len)(DISC_BUILTIN_ENDPOINT_SUBSCRIPTIONS_ANNOUNCER):= '1'; + end if; + if (NUM_WRITERS > 0) then + ret.data(ind+len)(DISC_BUILTIN_ENDPOINT_PUBLICATIONS_ANNOUNCER) := '1'; + ret.data(ind+len)(DISC_BUILTIN_ENDPOINT_SUBSCRIPTIONS_DETECTOR) := '1'; + end if; + -- MANUAL LIVELINESS COUNT + len := len + 1; + ret.data(ind+len) := PID_PARTICIPANT_MANUAL_LIVELINESS_COUNT & std_logic_vector(to_unsigned(4, 16)); + len := len + 1; + ret.data(ind+len) := (others => '0'); + -- SENTINEL + len := len + 1; + ret.data(ind+len) := PID_SENTINEL & std_logic_vector(to_unsigned(0, 16)); + + -- Store Length + ret.length := ind + len + 1; + return ret; + end function; + + constant PARTICIPANT_DATA : OUTPUT_DATA_TYPE := gen_participant_data; end package body;