diff --git a/src/ros2/Tests/Level_1/L1_AddTwoInts_ros_srv_test1.vhd b/src/ros2/Tests/Level_1/L1_AddTwoInts_ros_srv_test1.vhd index c4b8880..411bb8c 100644 --- a/src/ros2/Tests/Level_1/L1_AddTwoInts_ros_srv_test1.vhd +++ b/src/ros2/Tests/Level_1/L1_AddTwoInts_ros_srv_test1.vhd @@ -9,7 +9,7 @@ use work.rtps_package.all; use work.ros_package.all; use work.rtps_test_package.all; --- This testbench tests the General Behavour of the AddTwoInts Service. +-- This testbench tests the General Behavour of ROS Services. -- More specifically following tests are done: -- * Test Unssuported Opcode Operations -- * Test RETCODE_NO_DATA response from DDS Reader diff --git a/src/ros2/Tests/Level_1/L1_AddTwoInts_ros_srv_test2.vhd b/src/ros2/Tests/Level_1/L1_AddTwoInts_ros_srv_test2.vhd index 80dee0c..32f1e89 100644 --- a/src/ros2/Tests/Level_1/L1_AddTwoInts_ros_srv_test2.vhd +++ b/src/ros2/Tests/Level_1/L1_AddTwoInts_ros_srv_test2.vhd @@ -9,7 +9,7 @@ use work.rtps_package.all; use work.ros_package.all; use work.rtps_test_package.all; --- This testbench tests the General Behavour of the AddTwoInts Service. +-- This testbench tests the General Behavour of ROS Services. -- More specifically following tests are done: -- * Test Unssuported Opcode Operations -- * Test RETCODE_NO_DATA response from DDS Reader diff --git a/src/ros2/Tests/Level_1/L1_CancelGoal_ros_srv_test1.vhd b/src/ros2/Tests/Level_1/L1_CancelGoal_ros_srv_test1.vhd index 7e1cd8a..d7c8379 100644 --- a/src/ros2/Tests/Level_1/L1_CancelGoal_ros_srv_test1.vhd +++ b/src/ros2/Tests/Level_1/L1_CancelGoal_ros_srv_test1.vhd @@ -10,7 +10,7 @@ use work.ros_package.all; use work.rtps_test_package.all; use work.CancelGoal_package.all; --- This testbench tests the General Behavour of the AddTwoInts Service. +-- This testbench tests the General Behavour of the ROS Services. -- More specifically following tests are done: -- * Test Unssuported Opcode Operations -- * Test RETCODE_NO_DATA response from DDS Reader diff --git a/src/ros2/Tests/Level_1/L1_CancelGoal_ros_srv_test2.vhd b/src/ros2/Tests/Level_1/L1_CancelGoal_ros_srv_test2.vhd index 63e9644..9e3b7fe 100644 --- a/src/ros2/Tests/Level_1/L1_CancelGoal_ros_srv_test2.vhd +++ b/src/ros2/Tests/Level_1/L1_CancelGoal_ros_srv_test2.vhd @@ -10,7 +10,7 @@ use work.ros_package.all; use work.rtps_test_package.all; use work.CancelGoal_package.all; --- This testbench tests the General Behavour of the AddTwoInts Service. +-- This testbench tests the General Behavour of ROS Services. -- More specifically following tests are done: -- * Test Unssuported Opcode Operations -- * Test RETCODE_NO_DATA response from DDS Reader diff --git a/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_feedback_test1.vhd b/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_feedback_test1.vhd index 90cacdb..fba7ea9 100644 --- a/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_feedback_test1.vhd +++ b/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_feedback_test1.vhd @@ -36,7 +36,7 @@ architecture testbench of L1_Fibonacci_ros_action_feedback_test1 is signal message_info_sub : MESSAGE_INFO_TYPE := EMPTY_MESSAGE_INFO; -- signal goal_id_sub, goal_id_pub : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0'); - signal seq_len_sub, seq_len_pub, seq_addr_sub, seq_addr_pub : std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0) := (others => '0'); + signal seq_len_sub, seq_len_pub, seq_addr_sub, seq_addr_pub : std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0) := (others => '0'); signal seq_ready_sub, seq_ready_pub, seq_ren_sub, seq_ren_pub, seq_wen_pub, seq_valid_sub, seq_valid_pub, seq_ack_sub, seq_ack_pub : std_logic := '0'; signal seq_sub, seq_r_pub, seq_w_pub : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0'); begin diff --git a/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_feedback_test2.vhd b/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_feedback_test2.vhd index bf844c7..10463f4 100644 --- a/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_feedback_test2.vhd +++ b/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_feedback_test2.vhd @@ -36,7 +36,7 @@ architecture testbench of L1_Fibonacci_ros_action_feedback_test2 is signal message_info_sub : MESSAGE_INFO_TYPE := EMPTY_MESSAGE_INFO; -- signal goal_id_sub, goal_id_pub : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0'); - signal seq_len_sub, seq_len_pub, seq_addr_sub, seq_addr_pub : std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0) := (others => '0'); + signal seq_len_sub, seq_len_pub, seq_addr_sub, seq_addr_pub : std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0) := (others => '0'); signal seq_ready_sub, seq_ready_pub, seq_ren_sub, seq_ren_pub, seq_wen_pub, seq_valid_sub, seq_valid_pub, seq_ack_sub, seq_ack_pub : std_logic := '0'; signal seq_sub, seq_r_pub, seq_w_pub : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0'); begin diff --git a/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_goal_srv_test1.vhd b/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_goal_srv_test1.vhd new file mode 100644 index 0000000..7acb0e1 --- /dev/null +++ b/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_goal_srv_test1.vhd @@ -0,0 +1,437 @@ +library ieee; +use ieee.std_logic_1164.all; +use ieee.numeric_std.all; + +library osvvm; -- Utility Library +context osvvm.OsvvmContext; + +use work.rtps_package.all; +use work.ros_package.all; +use work.rtps_test_package.all; + +-- This testbench tests the General Behavour of ROS Services. +-- More specifically following tests are done: +-- * Test Unssuported Opcode Operations +-- * Test RETCODE_NO_DATA response from DDS Reader +-- * Test RETCODE_ERROR response from DDS Reader +-- * Test RETCODE_ERROR response from DDS Writer +-- * Test Sample with No Valid response from DDS Reader +-- * Test Big Endian Encoding/Decoding of AddTwoInts Service Messages + +entity L1_Fibonacci_ros_action_goal_srv_test1 is +end entity; + +architecture testbench of L1_Fibonacci_ros_action_goal_srv_test1 is + + signal clk, reset : std_logic := '0'; + signal valid_rq, valid_rr, ready_rq, ready_rr, last_word_rq, last_word_rr : std_logic := '0'; + signal data_rq, data_rr : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0'); + signal start_c, start_s, ack_c, ack_s, taken_c, taken_s, done_c, done_s : std_logic := '0'; + signal opcode_c, opcode_s : ROS_SERVICE_OPCODE_TYPE := NOP; + signal return_code_c, return_code_s : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0'); + signal service_info_c, service_info_s : SERVICE_INFO_TYPE := EMPTY_SERVICE_INFO; + signal sequence_id_c : std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0) := (others => '0'); + signal request_id_s : REQUEST_ID_TYPE := EMPTY_REQUEST_ID; + signal start_sr, start_cr, si_valid_data_cr, si_valid_data_sr : std_logic := '0'; + signal return_code_cr, return_code_cw, return_code_sr, return_code_sw : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0) := (others => '0'); + signal ready_sw, ready_cw, last_word_out_cw, last_word_out_sw, selector : std_logic := '0'; + -- + signal goal_id_c, goal_id_s : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0'); + signal order_c, order_s : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0'); + signal accepted_c, accepted_s : std_logic := '0'; + signal stamp_c, stamp_s : std_logic_vector(ROS_TIME_WIDTH-1 downto 0) := (others => '0'); +begin + + uut_c : entity work.Fibonacci_ros_action_goal_srv_client(arch) + port map ( + clk => clk, + reset => reset, + start_r => start_cr,-- + ack_r => '1',-- + opcode_r => open, + instance_state_r => open, + view_state_r => open, + sample_state_r => open, + instance_handle_r => open, + max_samples_r => open, + get_data_r => open,-- + done_r => '1',-- + return_code_r => return_code_cr,-- + valid_in_r => valid_rr,-- + ready_in_r => ready_rr,-- + data_in_r => data_rr,-- + last_word_in_r => last_word_rr,-- + si_sample_state_r => ANY_SAMPLE_STATE, + si_view_state_r => ANY_VIEW_STATE, + si_instance_state_r => ANY_INSTANCE_STATE, + si_source_timestamp_r => TIME_INVALID, + si_instance_handle_r => HANDLE_NIL, + si_publication_handle_r => HANDLE_NIL, + si_disposed_generation_count_r => (others => '0'), + si_no_writers_generation_count_r => (others => '0'), + si_sample_rank_r => (others => '0'), + si_generation_rank_r => (others => '0'), + si_absolute_generation_rank_r => (others => '0'), + si_valid_data_r => si_valid_data_cr,-- + si_valid_r => '1',-- + si_ack_r => open,-- + eoc_r => '1',-- + status_r => (others => '0'), + start_w => open,-- + ack_w => '1',-- + opcode_w => open, + instance_handle_out_w => open, + source_ts_w => open, + max_wait_w => open, + done_w => '1',-- + return_code_w => return_code_cw, + instance_handle_in_w => HANDLE_NIL, + valid_out_w => valid_rq,-- + ready_out_w => ready_cw,--ready_rq,-- + data_out_w => data_rq,-- + last_word_out_w => last_word_out_cw,--last_word_rq,-- + valid_in_w => '0', + ready_in_w => open, + data_in_w => (others => '0'), + last_word_in_w => '0', + status_w => (others => '0'), + start_user => start_c,-- + ack_user => ack_c,-- + opcode_user => opcode_c,-- + service_info_user => service_info_c,-- + sequence_id_user => sequence_id_c,-- + taken_user => taken_c,-- + data_available_user => open,-- + goal_id => goal_id_c, + order => order_c, + accepted => accepted_c, + stamp => stamp_c, + done_user => done_c,-- + return_code_user => return_code_c -- + ); + + uut_s : entity work.Fibonacci_ros_action_goal_srv_server(arch) + port map ( + clk => clk, + reset => reset, + start_r => start_sr,-- + ack_r => '1',-- + opcode_r => open, + instance_state_r => open, + view_state_r => open, + sample_state_r => open, + instance_handle_r => open, + max_samples_r => open, + get_data_r => open,-- + done_r => '1',-- + return_code_r => return_code_sr,-- + valid_in_r => valid_rq,-- + ready_in_r => ready_rq,-- + data_in_r => data_rq,-- + last_word_in_r => last_word_rq,-- + si_sample_state_r => ANY_SAMPLE_STATE, + si_view_state_r => ANY_VIEW_STATE, + si_instance_state_r => ANY_INSTANCE_STATE, + si_source_timestamp_r => TIME_INVALID, + si_instance_handle_r => HANDLE_NIL, + si_publication_handle_r => HANDLE_NIL, + si_disposed_generation_count_r => (others => '0'), + si_no_writers_generation_count_r => (others => '0'), + si_sample_rank_r => (others => '0'), + si_generation_rank_r => (others => '0'), + si_absolute_generation_rank_r => (others => '0'), + si_valid_data_r => si_valid_data_sr,-- + si_valid_r => '1',-- + si_ack_r => open,-- + eoc_r => '1',-- + status_r => (others => '0'), + start_w => open,-- + ack_w => '1',-- + opcode_w => open, + instance_handle_out_w => open, + source_ts_w => open, + max_wait_w => open, + done_w => '1',-- + return_code_w => return_code_sw,-- + instance_handle_in_w => HANDLE_NIL, + valid_out_w => valid_rr,-- + ready_out_w => ready_sw,--ready_rr,-- + data_out_w => data_rr,-- + last_word_out_w => last_word_out_sw,--last_word_rr,-- + valid_in_w => '0', + ready_in_w => open, + data_in_w => (others => '0'), + last_word_in_w => '0', + status_w => (others => '0'), + start_user => start_s,-- + ack_user => ack_s,-- + opcode_user => opcode_s,-- + service_info_user => service_info_s,-- + request_id_user => request_id_s,-- + taken_user => taken_s,-- + data_available_user => open,-- + goal_id => goal_id_s, + order => order_s, + accepted => accepted_s, + stamp => stamp_s, + done_user => done_s,-- + return_code_user => return_code_s -- + ); + + process (all) + begin + if (selector = '1') then + ready_cw <= '1'; + ready_sw <= '1'; + last_word_rq <= '0'; + last_word_rr <= '0'; + else + ready_cw <= ready_rq; + ready_sw <= ready_rr; + last_word_rq <= last_word_out_cw; + last_word_rr <= last_word_out_sw; + end if; + end process; + + stimulus_prc : process + variable RV : RandomPType; + variable GOAL_ID, ORDER, ACCEPTED, STAMP : AlertLogIDType; + + procedure wait_on_sig(signal sig : std_logic) is + begin + if (sig /= '1') then + wait on sig until sig = '1'; + end if; + end procedure; + begin + + SetAlertLogName("L1_Fibonacci_ros_action_goal_srv_test1 - (Big Endian) - General"); + SetAlertEnable(FAILURE, TRUE); + SetAlertEnable(ERROR, TRUE); + SetAlertEnable(WARNING, TRUE); + SetLogEnable(DEBUG, FALSE); + SetLogEnable(PASSED, FALSE); + SetLogEnable(INFO, TRUE); + RV.InitSeed(RV'instance_name); + GOAL_ID := GetAlertLogID("GOAL_ID", ALERTLOG_BASE_ID); + ORDER := GetAlertLogID("ORDER", ALERTLOG_BASE_ID); + ACCEPTED := GetAlertLogID("ACCEPTED", ALERTLOG_BASE_ID); + STAMP := GetAlertLogID("STAMP", ALERTLOG_BASE_ID); + + Log("Initial Reset", INFO); + selector <= '1'; + return_code_cr <= RETCODE_OK; + return_code_cw <= RETCODE_OK; + return_code_sr <= RETCODE_OK; + return_code_sw <= RETCODE_OK; + si_valid_data_cr <= '0'; + si_valid_data_sr <= '0'; + start_c <= '0'; + start_s <= '0'; + reset <= '1'; + wait until rising_edge(clk); + wait until rising_edge(clk); + reset <= '0'; + + Log("CLIENT: Test Unsupported Opcode", INFO); + start_c <= '1'; + opcode_c <= TAKE_REQUEST; + wait_on_sig(ack_c); + wait until rising_edge(clk); + start_c <= '0'; + wait_on_sig(done_c); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code_c /= ROS_RET_UNSUPPORTED, "Unexpected Client Response", FAILURE); + wait until rising_edge(clk); + + Log("SERVER: Test Unsupported Opcode", INFO); + start_s <= '1'; + opcode_s <= TAKE_RESPONSE; + wait_on_sig(ack_s); + wait until rising_edge(clk); + start_s <= '0'; + wait_on_sig(done_s); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code_s /= ROS_RET_UNSUPPORTED, "Unexpected Server Response", FAILURE); + wait until rising_edge(clk); + + Log("CLIENT: Test No Data", INFO); + return_code_cr <= RETCODE_NO_DATA; + start_c <= '1'; + opcode_c <= TAKE_RESPONSE; + wait_on_sig(ack_c); + wait until rising_edge(clk); + start_c <= '0'; + wait_on_sig(done_c); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code_c /= ROS_RET_OK, "Unexpected Client Response", FAILURE); + AlertIf(taken_c /= '0', "Client taken is unexpectedly set", FAILURE); + wait until rising_edge(clk); + + Log("SERVER: Test No Data", INFO); + return_code_sr <= RETCODE_NO_DATA; + start_s <= '1'; + opcode_s <= TAKE_REQUEST; + wait_on_sig(ack_s); + wait until rising_edge(clk); + start_s <= '0'; + wait_on_sig(done_s); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code_s /= ROS_RET_OK, "Unexpected Server Response", FAILURE); + AlertIf(taken_s /= '0', "Server taken is unexpectedly set", FAILURE); + wait until rising_edge(clk); + + Log("CLIENT: Test Reader Error", INFO); + return_code_cr <= RETCODE_ERROR; + start_c <= '1'; + opcode_c <= TAKE_RESPONSE; + wait_on_sig(ack_c); + wait until rising_edge(clk); + start_c <= '0'; + wait_on_sig(done_c); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code_c /= ROS_RET_ERROR, "Unexpected Client Response", FAILURE); + wait until rising_edge(clk); + + Log("SERVER: Test Reader Error", INFO); + return_code_sr <= RETCODE_ERROR; + start_s <= '1'; + opcode_s <= TAKE_REQUEST; + wait_on_sig(ack_s); + wait until rising_edge(clk); + start_s <= '0'; + wait_on_sig(done_s); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code_s /= ROS_RET_ERROR, "Unexpected Server Response", FAILURE); + wait until rising_edge(clk); + + Log("CLIENT: Test Writer Error", INFO); + return_code_cw <= RETCODE_ERROR; + start_c <= '1'; + opcode_c <= SEND_REQUEST; + wait_on_sig(ack_c); + wait until rising_edge(clk); + start_c <= '0'; + wait_on_sig(done_c); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code_c /= ROS_RET_ERROR, "Unexpected Client Response", FAILURE); + wait until rising_edge(clk); + + Log("SERVER: Test Writer Error", INFO); + return_code_sw <= RETCODE_ERROR; + start_s <= '1'; + opcode_s <= SEND_RESPONSE; + wait_on_sig(ack_s); + wait until rising_edge(clk); + start_s <= '0'; + wait_on_sig(done_s); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code_s /= ROS_RET_ERROR, "Unexpected Server Response", FAILURE); + wait until rising_edge(clk); + + return_code_cr <= RETCODE_OK; + return_code_cw <= RETCODE_OK; + return_code_sr <= RETCODE_OK; + return_code_sw <= RETCODE_OK; + selector <= '0'; + + Log("Setting Request", INFO); + -- Static + goal_id_c <= RV.RandSlv(goal_id_c'length); + order_c <= RV.RandSlv(order_c'length); + wait for 0 ns; + + Log("CLIENT: Send Request", INFO); + start_c <= '1'; + opcode_c <= SEND_REQUEST; + wait_on_sig(ack_c); + wait until rising_edge(clk); + start_c <= '0'; + + Log("SERVER: Take Request", INFO); + start_s <= '1'; + opcode_s <= TAKE_REQUEST; + wait_on_sig(ack_s); + wait until rising_edge(clk); + start_s <= '0'; + + -- TEST NO VALID DATA + wait_on_sig(start_sr); + wait until rising_edge(clk); + wait until rising_edge(clk); + wait_on_sig(start_sr); + si_valid_data_sr <= '1'; + + Log("Wait for Request on Server", INFO); + wait_on_sig(done_s); + wait until rising_edge(clk); + + AlertIf(return_code_s /= ROS_RET_OK, "Server did Return ERROR", FAILURE); + AlertIf(taken_s /= '1', "Server did not take Request", FAILURE); + + Log("Compare Request", INFO); + AffirmIfEqual(GOAL_ID, goal_id_s, goal_id_c); + AffirmIfEqual(ORDER, order_s, order_c); + + Log("Setting Response", INFO); + -- Static + accepted_s <= RV.RandSlv(1)(1); + stamp_s <= RV.RandSlv(stamp_s'length); + wait for 0 ns; + + Log("SERVER: Send Response", INFO); + start_s <= '1'; + opcode_s <= SEND_RESPONSE; + wait_on_sig(ack_s); + wait until rising_edge(clk); + start_s <= '0'; + + Log("CLIENT: Take Response", INFO); + start_c <= '1'; + opcode_c <= TAKE_RESPONSE; + wait_on_sig(ack_c); + wait until rising_edge(clk); + start_c <= '0'; + + -- TEST NO VALID DATA + wait_on_sig(start_cr); + wait until rising_edge(clk); + wait until rising_edge(clk); + wait_on_sig(start_cr); + si_valid_data_cr <= '1'; + + Log("Wait for Response on Client", INFO); + wait_on_sig(done_c); + wait until rising_edge(clk); + + AlertIf(return_code_c /= ROS_RET_OK, "Client did Return ERROR", FAILURE); + AlertIf(taken_c /= '1', "Client did not take Response", FAILURE); + + Log("Compare Response", INFO); + AffirmIfEqual(ACCEPTED, accepted_c, accepted_s); + AffirmIfEqual(STAMP, stamp_c, stamp_s); + + TranscriptOpen(RESULTS_FILE, APPEND_MODE); + SetTranscriptMirror; + ReportAlerts; + TranscriptClose; + std.env.stop; + wait; + end process; + + clock_prc : process + begin + clk <= '0'; + wait for TEST_CLOCK_PERIOD/2; + clk <= '1'; + wait for TEST_CLOCK_PERIOD/2; + end process; + + watchdog : process + begin + wait for 1 ms; + Alert("Test timeout", FAILURE); + std.env.stop; + end process; + +end architecture; \ No newline at end of file diff --git a/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_goal_srv_test2.vhd b/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_goal_srv_test2.vhd new file mode 100644 index 0000000..547b871 --- /dev/null +++ b/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_goal_srv_test2.vhd @@ -0,0 +1,443 @@ +library ieee; +use ieee.std_logic_1164.all; +use ieee.numeric_std.all; + +library osvvm; -- Utility Library +context osvvm.OsvvmContext; + +use work.rtps_package.all; +use work.ros_package.all; +use work.rtps_test_package.all; + +-- This testbench tests the General Behavour of ROS Services. +-- More specifically following tests are done: +-- * Test Unssuported Opcode Operations +-- * Test RETCODE_NO_DATA response from DDS Reader +-- * Test RETCODE_ERROR response from DDS Reader +-- * Test RETCODE_ERROR response from DDS Writer +-- * Test Sample with No Valid response from DDS Reader +-- * Test Little Endian Encoding/Decoding of AddTwoInts Service Messages + +entity L1_Fibonacci_ros_action_goal_srv_test2 is +end entity; + +architecture testbench of L1_Fibonacci_ros_action_goal_srv_test2 is + + signal clk, reset : std_logic := '0'; + signal valid_rq, valid_rr, ready_rq, ready_rr, last_word_rq, last_word_rr : std_logic := '0'; + signal data_rq, data_rr : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0'); + signal start_c, start_s, ack_c, ack_s, taken_c, taken_s, done_c, done_s : std_logic := '0'; + signal opcode_c, opcode_s : ROS_SERVICE_OPCODE_TYPE := NOP; + signal return_code_c, return_code_s : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0'); + signal service_info_c, service_info_s : SERVICE_INFO_TYPE := EMPTY_SERVICE_INFO; + signal sequence_id_c : std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0) := (others => '0'); + signal request_id_s : REQUEST_ID_TYPE := EMPTY_REQUEST_ID; + signal start_sr, start_cr, si_valid_data_cr, si_valid_data_sr : std_logic := '0'; + signal return_code_cr, return_code_cw, return_code_sr, return_code_sw : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0) := (others => '0'); + signal ready_sw, ready_cw, last_word_out_cw, last_word_out_sw, selector : std_logic := '0'; + -- + signal goal_id_c, goal_id_s : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0'); + signal order_c, order_s : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0'); + signal accepted_c, accepted_s : std_logic := '0'; + signal stamp_c, stamp_s : std_logic_vector(ROS_TIME_WIDTH-1 downto 0) := (others => '0'); +begin + + uut_c : entity work.Fibonacci_ros_action_goal_srv_client(arch) + generic map ( + LITTLE_ENDIAN => '1' + ) + port map ( + clk => clk, + reset => reset, + start_r => start_cr,-- + ack_r => '1',-- + opcode_r => open, + instance_state_r => open, + view_state_r => open, + sample_state_r => open, + instance_handle_r => open, + max_samples_r => open, + get_data_r => open,-- + done_r => '1',-- + return_code_r => return_code_cr,-- + valid_in_r => valid_rr,-- + ready_in_r => ready_rr,-- + data_in_r => data_rr,-- + last_word_in_r => last_word_rr,-- + si_sample_state_r => ANY_SAMPLE_STATE, + si_view_state_r => ANY_VIEW_STATE, + si_instance_state_r => ANY_INSTANCE_STATE, + si_source_timestamp_r => TIME_INVALID, + si_instance_handle_r => HANDLE_NIL, + si_publication_handle_r => HANDLE_NIL, + si_disposed_generation_count_r => (others => '0'), + si_no_writers_generation_count_r => (others => '0'), + si_sample_rank_r => (others => '0'), + si_generation_rank_r => (others => '0'), + si_absolute_generation_rank_r => (others => '0'), + si_valid_data_r => si_valid_data_cr,-- + si_valid_r => '1',-- + si_ack_r => open,-- + eoc_r => '1',-- + status_r => (others => '0'), + start_w => open,-- + ack_w => '1',-- + opcode_w => open, + instance_handle_out_w => open, + source_ts_w => open, + max_wait_w => open, + done_w => '1',-- + return_code_w => return_code_cw, + instance_handle_in_w => HANDLE_NIL, + valid_out_w => valid_rq,-- + ready_out_w => ready_cw,--ready_rq,-- + data_out_w => data_rq,-- + last_word_out_w => last_word_out_cw,--last_word_rq,-- + valid_in_w => '0', + ready_in_w => open, + data_in_w => (others => '0'), + last_word_in_w => '0', + status_w => (others => '0'), + start_user => start_c,-- + ack_user => ack_c,-- + opcode_user => opcode_c,-- + service_info_user => service_info_c,-- + sequence_id_user => sequence_id_c,-- + taken_user => taken_c,-- + data_available_user => open,-- + goal_id => goal_id_c, + order => order_c, + accepted => accepted_c, + stamp => stamp_c, + done_user => done_c,-- + return_code_user => return_code_c -- + ); + + uut_s : entity work.Fibonacci_ros_action_goal_srv_server(arch) + generic map ( + LITTLE_ENDIAN => '1' + ) + port map ( + clk => clk, + reset => reset, + start_r => start_sr,-- + ack_r => '1',-- + opcode_r => open, + instance_state_r => open, + view_state_r => open, + sample_state_r => open, + instance_handle_r => open, + max_samples_r => open, + get_data_r => open,-- + done_r => '1',-- + return_code_r => return_code_sr,-- + valid_in_r => valid_rq,-- + ready_in_r => ready_rq,-- + data_in_r => data_rq,-- + last_word_in_r => last_word_rq,-- + si_sample_state_r => ANY_SAMPLE_STATE, + si_view_state_r => ANY_VIEW_STATE, + si_instance_state_r => ANY_INSTANCE_STATE, + si_source_timestamp_r => TIME_INVALID, + si_instance_handle_r => HANDLE_NIL, + si_publication_handle_r => HANDLE_NIL, + si_disposed_generation_count_r => (others => '0'), + si_no_writers_generation_count_r => (others => '0'), + si_sample_rank_r => (others => '0'), + si_generation_rank_r => (others => '0'), + si_absolute_generation_rank_r => (others => '0'), + si_valid_data_r => si_valid_data_sr,-- + si_valid_r => '1',-- + si_ack_r => open,-- + eoc_r => '1',-- + status_r => (others => '0'), + start_w => open,-- + ack_w => '1',-- + opcode_w => open, + instance_handle_out_w => open, + source_ts_w => open, + max_wait_w => open, + done_w => '1',-- + return_code_w => return_code_sw,-- + instance_handle_in_w => HANDLE_NIL, + valid_out_w => valid_rr,-- + ready_out_w => ready_sw,--ready_rr,-- + data_out_w => data_rr,-- + last_word_out_w => last_word_out_sw,--last_word_rr,-- + valid_in_w => '0', + ready_in_w => open, + data_in_w => (others => '0'), + last_word_in_w => '0', + status_w => (others => '0'), + start_user => start_s,-- + ack_user => ack_s,-- + opcode_user => opcode_s,-- + service_info_user => service_info_s,-- + request_id_user => request_id_s,-- + taken_user => taken_s,-- + data_available_user => open,-- + goal_id => goal_id_s, + order => order_s, + accepted => accepted_s, + stamp => stamp_s, + done_user => done_s,-- + return_code_user => return_code_s -- + ); + + process (all) + begin + if (selector = '1') then + ready_cw <= '1'; + ready_sw <= '1'; + last_word_rq <= '0'; + last_word_rr <= '0'; + else + ready_cw <= ready_rq; + ready_sw <= ready_rr; + last_word_rq <= last_word_out_cw; + last_word_rr <= last_word_out_sw; + end if; + end process; + + stimulus_prc : process + variable RV : RandomPType; + variable GOAL_ID, ORDER, ACCEPTED, STAMP : AlertLogIDType; + + procedure wait_on_sig(signal sig : std_logic) is + begin + if (sig /= '1') then + wait on sig until sig = '1'; + end if; + end procedure; + begin + + SetAlertLogName("L1_Fibonacci_ros_action_goal_srv_test2 - (Little Endian) - General"); + SetAlertEnable(FAILURE, TRUE); + SetAlertEnable(ERROR, TRUE); + SetAlertEnable(WARNING, TRUE); + SetLogEnable(DEBUG, FALSE); + SetLogEnable(PASSED, FALSE); + SetLogEnable(INFO, TRUE); + RV.InitSeed(RV'instance_name); + GOAL_ID := GetAlertLogID("GOAL_ID", ALERTLOG_BASE_ID); + ORDER := GetAlertLogID("ORDER", ALERTLOG_BASE_ID); + ACCEPTED := GetAlertLogID("ACCEPTED", ALERTLOG_BASE_ID); + STAMP := GetAlertLogID("STAMP", ALERTLOG_BASE_ID); + + Log("Initial Reset", INFO); + selector <= '1'; + return_code_cr <= RETCODE_OK; + return_code_cw <= RETCODE_OK; + return_code_sr <= RETCODE_OK; + return_code_sw <= RETCODE_OK; + si_valid_data_cr <= '0'; + si_valid_data_sr <= '0'; + start_c <= '0'; + start_s <= '0'; + reset <= '1'; + wait until rising_edge(clk); + wait until rising_edge(clk); + reset <= '0'; + + Log("CLIENT: Test Unsupported Opcode", INFO); + start_c <= '1'; + opcode_c <= TAKE_REQUEST; + wait_on_sig(ack_c); + wait until rising_edge(clk); + start_c <= '0'; + wait_on_sig(done_c); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code_c /= ROS_RET_UNSUPPORTED, "Unexpected Client Response", FAILURE); + wait until rising_edge(clk); + + Log("SERVER: Test Unsupported Opcode", INFO); + start_s <= '1'; + opcode_s <= TAKE_RESPONSE; + wait_on_sig(ack_s); + wait until rising_edge(clk); + start_s <= '0'; + wait_on_sig(done_s); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code_s /= ROS_RET_UNSUPPORTED, "Unexpected Server Response", FAILURE); + wait until rising_edge(clk); + + Log("CLIENT: Test No Data", INFO); + return_code_cr <= RETCODE_NO_DATA; + start_c <= '1'; + opcode_c <= TAKE_RESPONSE; + wait_on_sig(ack_c); + wait until rising_edge(clk); + start_c <= '0'; + wait_on_sig(done_c); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code_c /= ROS_RET_OK, "Unexpected Client Response", FAILURE); + AlertIf(taken_c /= '0', "Client taken is unexpectedly set", FAILURE); + wait until rising_edge(clk); + + Log("SERVER: Test No Data", INFO); + return_code_sr <= RETCODE_NO_DATA; + start_s <= '1'; + opcode_s <= TAKE_REQUEST; + wait_on_sig(ack_s); + wait until rising_edge(clk); + start_s <= '0'; + wait_on_sig(done_s); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code_s /= ROS_RET_OK, "Unexpected Server Response", FAILURE); + AlertIf(taken_s /= '0', "Server taken is unexpectedly set", FAILURE); + wait until rising_edge(clk); + + Log("CLIENT: Test Reader Error", INFO); + return_code_cr <= RETCODE_ERROR; + start_c <= '1'; + opcode_c <= TAKE_RESPONSE; + wait_on_sig(ack_c); + wait until rising_edge(clk); + start_c <= '0'; + wait_on_sig(done_c); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code_c /= ROS_RET_ERROR, "Unexpected Client Response", FAILURE); + wait until rising_edge(clk); + + Log("SERVER: Test Reader Error", INFO); + return_code_sr <= RETCODE_ERROR; + start_s <= '1'; + opcode_s <= TAKE_REQUEST; + wait_on_sig(ack_s); + wait until rising_edge(clk); + start_s <= '0'; + wait_on_sig(done_s); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code_s /= ROS_RET_ERROR, "Unexpected Server Response", FAILURE); + wait until rising_edge(clk); + + Log("CLIENT: Test Writer Error", INFO); + return_code_cw <= RETCODE_ERROR; + start_c <= '1'; + opcode_c <= SEND_REQUEST; + wait_on_sig(ack_c); + wait until rising_edge(clk); + start_c <= '0'; + wait_on_sig(done_c); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code_c /= ROS_RET_ERROR, "Unexpected Client Response", FAILURE); + wait until rising_edge(clk); + + Log("SERVER: Test Writer Error", INFO); + return_code_sw <= RETCODE_ERROR; + start_s <= '1'; + opcode_s <= SEND_RESPONSE; + wait_on_sig(ack_s); + wait until rising_edge(clk); + start_s <= '0'; + wait_on_sig(done_s); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code_s /= ROS_RET_ERROR, "Unexpected Server Response", FAILURE); + wait until rising_edge(clk); + + return_code_cr <= RETCODE_OK; + return_code_cw <= RETCODE_OK; + return_code_sr <= RETCODE_OK; + return_code_sw <= RETCODE_OK; + selector <= '0'; + + Log("Setting Request", INFO); + -- Static + goal_id_c <= RV.RandSlv(goal_id_c'length); + order_c <= RV.RandSlv(order_c'length); + wait for 0 ns; + + Log("CLIENT: Send Request", INFO); + start_c <= '1'; + opcode_c <= SEND_REQUEST; + wait_on_sig(ack_c); + wait until rising_edge(clk); + start_c <= '0'; + + Log("SERVER: Take Request", INFO); + start_s <= '1'; + opcode_s <= TAKE_REQUEST; + wait_on_sig(ack_s); + wait until rising_edge(clk); + start_s <= '0'; + + -- TEST NO VALID DATA + wait_on_sig(start_sr); + wait until rising_edge(clk); + wait until rising_edge(clk); + wait_on_sig(start_sr); + si_valid_data_sr <= '1'; + + Log("Wait for Request on Server", INFO); + wait_on_sig(done_s); + wait until rising_edge(clk); + + AlertIf(return_code_s /= ROS_RET_OK, "Server did Return ERROR", FAILURE); + AlertIf(taken_s /= '1', "Server did not take Request", FAILURE); + + Log("Compare Request", INFO); + AffirmIfEqual(GOAL_ID, goal_id_s, goal_id_c); + AffirmIfEqual(ORDER, order_s, order_c); + + Log("Setting Response", INFO); + -- Static + accepted_s <= RV.RandSlv(1)(1); + stamp_s <= RV.RandSlv(stamp_s'length); + wait for 0 ns; + + Log("SERVER: Send Response", INFO); + start_s <= '1'; + opcode_s <= SEND_RESPONSE; + wait_on_sig(ack_s); + wait until rising_edge(clk); + start_s <= '0'; + + Log("CLIENT: Take Response", INFO); + start_c <= '1'; + opcode_c <= TAKE_RESPONSE; + wait_on_sig(ack_c); + wait until rising_edge(clk); + start_c <= '0'; + + -- TEST NO VALID DATA + wait_on_sig(start_cr); + wait until rising_edge(clk); + wait until rising_edge(clk); + wait_on_sig(start_cr); + si_valid_data_cr <= '1'; + + Log("Wait for Response on Client", INFO); + wait_on_sig(done_c); + wait until rising_edge(clk); + + AlertIf(return_code_c /= ROS_RET_OK, "Client did Return ERROR", FAILURE); + AlertIf(taken_c /= '1', "Client did not take Response", FAILURE); + + Log("Compare Response", INFO); + AffirmIfEqual(ACCEPTED, accepted_c, accepted_s); + AffirmIfEqual(STAMP, stamp_c, stamp_s); + + TranscriptOpen(RESULTS_FILE, APPEND_MODE); + SetTranscriptMirror; + ReportAlerts; + TranscriptClose; + std.env.stop; + wait; + end process; + + clock_prc : process + begin + clk <= '0'; + wait for TEST_CLOCK_PERIOD/2; + clk <= '1'; + wait for TEST_CLOCK_PERIOD/2; + end process; + + watchdog : process + begin + wait for 1 ms; + Alert("Test timeout", FAILURE); + std.env.stop; + end process; + +end architecture; \ No newline at end of file diff --git a/src/ros2/Tests/ros_testbench.pro b/src/ros2/Tests/ros_testbench.pro index 60980ad..df6d33e 100644 --- a/src/ros2/Tests/ros_testbench.pro +++ b/src/ros2/Tests/ros_testbench.pro @@ -93,11 +93,6 @@ analyze ../example_interfaces/AddTwoInts_ros_srv_server.vhd analyze Level_1/L1_AddTwoInts_ros_srv_test1.vhd analyze Level_1/L1_AddTwoInts_ros_srv_test2.vhd analyze Level_2/L2_AddTwoInts_ros_srv_test1.vhd -analyze ../example_interfaces/Fibonacci_package.vhd -analyze ../example_interfaces/Fibonacci_ros_action_feedback_pub.vhd -analyze ../example_interfaces/Fibonacci_ros_action_feedback_sub.vhd -analyze Level_1/L1_Fibonacci_ros_action_feedback_test1.vhd -analyze Level_1/L1_Fibonacci_ros_action_feedback_test2.vhd analyze ../rcl_interfaces/action_msgs/GoalInfo_package.vhd analyze ../rcl_interfaces/action_msgs/GoalStatus_package.vhd analyze ../rcl_interfaces/action_msgs/GoalStatusArray_package.vhd @@ -110,14 +105,25 @@ analyze ../rcl_interfaces/action_msgs/CancelGoal_ros_srv_server.vhd analyze ../rcl_interfaces/action_msgs/CancelGoal_ros_srv_client.vhd analyze Level_1/L1_CancelGoal_ros_srv_test1.vhd analyze Level_1/L1_CancelGoal_ros_srv_test2.vhd +analyze ../example_interfaces/Fibonacci_package.vhd +analyze ../example_interfaces/Fibonacci_ros_action_feedback_pub.vhd +analyze ../example_interfaces/Fibonacci_ros_action_feedback_sub.vhd +analyze Level_1/L1_Fibonacci_ros_action_feedback_test1.vhd +analyze Level_1/L1_Fibonacci_ros_action_feedback_test2.vhd +analyze ../example_interfaces/Fibonacci_ros_action_goal_srv_server.vhd +analyze ../example_interfaces/Fibonacci_ros_action_goal_srv_client.vhd +analyze Level_1/L1_Fibonacci_ros_action_goal_srv_test1.vhd +analyze Level_1/L1_Fibonacci_ros_action_goal_srv_test2.vhd simulate L1_AddTwoInts_ros_srv_test1 simulate L1_AddTwoInts_ros_srv_test2 simulate L2_AddTwoInts_ros_srv_test1 -simulate L1_Fibonacci_ros_action_feedback_test1 -simulate L1_Fibonacci_ros_action_feedback_test2 simulate L1_GoalStatusArray_ros_test1 simulate L1_GoalStatusArray_ros_test2 simulate L1_CancelGoal_ros_srv_test1 -simulate L1_CancelGoal_ros_srv_test2 \ No newline at end of file +simulate L1_CancelGoal_ros_srv_test2 +simulate L1_Fibonacci_ros_action_feedback_test1 +simulate L1_Fibonacci_ros_action_feedback_test2 +simulate L1_Fibonacci_ros_action_goal_srv_test1 +simulate L1_Fibonacci_ros_action_goal_srv_test2 \ No newline at end of file diff --git a/src/ros2/example_interfaces/Fibonacci_package.vhd b/src/ros2/example_interfaces/Fibonacci_package.vhd index fa7bbf5..8577318 100644 --- a/src/ros2/example_interfaces/Fibonacci_package.vhd +++ b/src/ros2/example_interfaces/Fibonacci_package.vhd @@ -4,16 +4,46 @@ use ieee.numeric_std.all; use work.math_pkg.all; use work.ros_package.all; +use work.GoalInfo_package; package Fibonacci_package is - constant GOAL_ID_MAX_DEPTH : natural := 16; - constant GOAL_ID_ADDR_WIDTH : natural := log2c(GOAL_ID_MAX_DEPTH); - constant SEQ_MAX_DEPTH : natural := 100; - constant SEQ_ADDR_WIDTH : natural := log2c(SEQ_MAX_DEPTH); + -- GOAL + constant G_RQ_GOAL_ID_MAX_DEPTH : natural := GoalInfo_package.GOAL_ID_MAX_DEPTH; + constant G_RQ_GOAL_ID_ADDR_WIDTH : natural := GoalInfo_package.GOAL_ID_ADDR_WIDTH; - constant MAX_GOAL_ID_SIZE : natural := GOAL_ID_MAX_DEPTH; - constant MAX_SEQ_SIZE : natural := SEQ_MAX_DEPTH * 4; - constant MAX_FIBONACCI_SIZE : natural := MAX_GOAL_ID_SIZE + MAX_SEQ_SIZE; + constant G_RQ_MAX_GOAL_ID_SIZE : natural := GoalInfo_package.MAX_GOAL_ID_SIZE; + constant G_RQ_MAX_ORDER_SIZE : natural := 4; + constant G_RQ_FIBONACCI_SIZE : natural := G_RQ_MAX_GOAL_ID_SIZE + G_RQ_MAX_ORDER_SIZE; + + constant G_RR_MAX_ACCEPTED : natural := 1; + constant G_RR_MAX_STAMP_SEC_SIZE : natural := GoalInfo_package.MAX_STAMP_SEC_SIZE; + constant G_RR_MAX_STAMP_NANOSEC_SIZE : natural := GoalInfo_package.MAX_STAMP_NANOSEC_SIZE; + constant G_RR_MAX_STAMP_SIZE : natural := GoalInfo_package.MAX_STAMP_SIZE; + constant G_RR_MAX_FIBONACCI_SIZE : natural := G_RR_MAX_ACCEPTED + G_RR_MAX_STAMP_SIZE; + + -- RESULT + constant R_RQ_GOAL_ID_MAX_DEPTH : natural := GoalInfo_package.GOAL_ID_MAX_DEPTH; + constant R_RQ_GOAL_ID_ADDR_WIDTH : natural := GoalInfo_package.GOAL_ID_ADDR_WIDTH; + + constant R_RQ_MAX_GOAL_ID_SIZE : natural := GoalInfo_package.MAX_GOAL_ID_SIZE; + constant R_RQ_MAX_FIBONACCI_SIZE : natural := R_RQ_MAX_GOAL_ID_SIZE; + + constant R_RR_SEQ_MAX_DEPTH : natural := 100; + constant R_RR_SEQ_ADDR_WIDTH : natural := log2c(R_RR_SEQ_MAX_DEPTH); + + constant R_RR_MAX_STATUS_SIZE : natural := 1; + constant R_RR_MAX_SEQ_SIZE : natural := R_RR_SEQ_MAX_DEPTH * 4; + constant R_RR_MAX_FIBONACCI_SIZE : natural := R_RR_MAX_STATUS_SIZE + R_RR_MAX_SEQ_SIZE; + + -- FEEDBACK + constant F_GOAL_ID_MAX_DEPTH : natural := GoalInfo_package.GOAL_ID_MAX_DEPTH; + constant F_GOAL_ID_ADDR_WIDTH : natural := GoalInfo_package.GOAL_ID_ADDR_WIDTH; + constant F_SEQ_MAX_DEPTH : natural := 100; + constant F_SEQ_ADDR_WIDTH : natural := log2c(F_SEQ_MAX_DEPTH); + + constant F_MAX_GOAL_ID_SIZE : natural := GoalInfo_package.MAX_GOAL_ID_SIZE; + constant F_MAX_SEQ_SIZE : natural := F_SEQ_MAX_DEPTH * 4; + constant F_MAX_FIBONACCI_SIZE : natural := F_MAX_GOAL_ID_SIZE + F_MAX_SEQ_SIZE; end package; \ No newline at end of file diff --git a/src/ros2/example_interfaces/Fibonacci_ros_action_feedback_pub.vhd b/src/ros2/example_interfaces/Fibonacci_ros_action_feedback_pub.vhd index f0de78a..515f6fe 100644 --- a/src/ros2/example_interfaces/Fibonacci_ros_action_feedback_pub.vhd +++ b/src/ros2/example_interfaces/Fibonacci_ros_action_feedback_pub.vhd @@ -46,8 +46,8 @@ entity Fibonacci_ros_action_feedback_pub is -- ###GENERATED START### goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0); - seq_len : in std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0); - seq_addr : in std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0); + seq_len : in std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0); + seq_addr : in std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0); seq_ready : out std_logic; seq_ren : in std_logic; seq_wen : in std_logic; @@ -83,10 +83,10 @@ architecture arch of Fibonacci_ros_action_feedback_pub is signal return_code_latch, return_code_latch_next : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); signal encode_stage, encode_stage_next : ENCODE_STAGE_TYPE; -- ###GENERATED START### - signal uuid_cnt, uuid_cnt_next : natural range 0 to GOAL_ID_MAX_DEPTH-1; - signal seq_cnt, seq_cnt_next : natural range 0 to SEQ_MAX_DEPTH-1; + signal uuid_cnt, uuid_cnt_next : natural range 0 to F_GOAL_ID_MAX_DEPTH-1; + signal seq_cnt, seq_cnt_next : natural range 0 to F_SEQ_MAX_DEPTH-1; -- seq_mem SIGNALS - signal seq_mem_addr : std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0); + signal seq_mem_addr : std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0); signal seq_mem_read, seq_mem_ready_in, seq_mem_ready_out, seq_mem_valid_in, seq_mem_valid_out : std_logic; signal seq_mem_data_in, seq_mem_data_out : std_logic_vector(CDR_LONG_WIDTH-1 downto 0); -- ###GENERATED END### @@ -97,9 +97,9 @@ begin -- ###GENERATED START### seq_mem : entity work.mem_ctrl(arch) generic map ( - ADDR_WIDTH => SEQ_ADDR_WIDTH, + ADDR_WIDTH => F_SEQ_ADDR_WIDTH, DATA_WIDTH => CDR_LONG_WIDTH, - MEMORY_DEPTH => SEQ_MAX_DEPTH, + MEMORY_DEPTH => F_SEQ_MAX_DEPTH, MAX_BURST_LENGTH => 1 ) port map ( @@ -262,7 +262,7 @@ begin data_out_latch_next <= write_sub_vector(data_out_latch_next, get_sub_vector(goal_id,uuid_cnt,CDR_INT8_WIDTH,TRUE), to_integer(align_offset(1 downto 0)), TRUE); align_offset_next <= align_offset + 1; - if (uuid_cnt = GOAL_ID_MAX_DEPTH-1) then + if (uuid_cnt = F_GOAL_ID_MAX_DEPTH-1) then encode_stage_next <= WRITE_SEQ_LENGTH; cnt_next <= 0; uuid_cnt_next <= 0; -- Post-Reset @@ -305,7 +305,7 @@ begin case (cnt) is -- GET when 0 => - seq_mem_addr <= std_logic_vector(to_unsigned(seq_cnt,SEQ_ADDR_WIDTH)); + seq_mem_addr <= std_logic_vector(to_unsigned(seq_cnt,F_SEQ_ADDR_WIDTH)); seq_mem_valid_in <= '1'; seq_mem_read <= '1'; -- Memory Operation Guard diff --git a/src/ros2/example_interfaces/Fibonacci_ros_action_feedback_sub.vhd b/src/ros2/example_interfaces/Fibonacci_ros_action_feedback_sub.vhd index c63c90e..b3af6be 100644 --- a/src/ros2/example_interfaces/Fibonacci_ros_action_feedback_sub.vhd +++ b/src/ros2/example_interfaces/Fibonacci_ros_action_feedback_sub.vhd @@ -62,8 +62,8 @@ entity Fibonacci_ros_action_feedback_sub is -- ###GENERATED START### goal_id : out std_logic_vector(UUID_WIDTH-1 downto 0); - seq_len : out std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0); - seq_addr : in std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0); + seq_len : out std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0); + seq_addr : in std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0); seq_ready : out std_logic; seq_ren : in std_logic; seq_valid : out std_logic; @@ -104,12 +104,12 @@ architecture arch of Fibonacci_ros_action_feedback_sub is signal decode_stage, decode_stage_next : DECODE_STAGE_TYPE; signal return_stage, return_stage_next : DECODE_STAGE_TYPE; -- ###GENERATED START### - signal uuid_cnt, uuid_cnt_next : natural range 0 to GOAL_ID_MAX_DEPTH-1; - signal seq_cnt, seq_cnt_next : natural range 0 to SEQ_MAX_DEPTH-1; - signal seq_len_latch, seq_len_latch_next : unsigned(SEQ_ADDR_WIDTH-1 downto 0); + signal uuid_cnt, uuid_cnt_next : natural range 0 to F_GOAL_ID_MAX_DEPTH-1; + signal seq_cnt, seq_cnt_next : natural range 0 to F_SEQ_MAX_DEPTH-1; + signal seq_len_latch, seq_len_latch_next : unsigned(F_SEQ_ADDR_WIDTH-1 downto 0); signal goal_id_latch, goal_id_latch_next : std_logic_vector(UUID_WIDTH-1 downto 0); -- seq_mem SIGNALS - signal seq_mem_addr : std_logic_vector(SEQ_ADDR_WIDTH-1 downto 0); + signal seq_mem_addr : std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0); signal seq_mem_read, seq_mem_ready_in, seq_mem_ready_out, seq_mem_valid_in, seq_mem_valid_out : std_logic; signal seq_mem_data_in, seq_mem_data_out : std_logic_vector(CDR_LONG_WIDTH-1 downto 0); -- ###GENERATED END### @@ -124,9 +124,9 @@ begin -- ###GENERATED START### seq_mem : entity work.mem_ctrl(arch) generic map ( - ADDR_WIDTH => SEQ_ADDR_WIDTH, + ADDR_WIDTH => F_SEQ_ADDR_WIDTH, DATA_WIDTH => CDR_LONG_WIDTH, - MEMORY_DEPTH => SEQ_MAX_DEPTH, + MEMORY_DEPTH => F_SEQ_MAX_DEPTH, MAX_BURST_LENGTH => 1 ) port map ( @@ -357,7 +357,7 @@ begin stage_next <= FETCH; end if; - if (uuid_cnt = GOAL_ID_MAX_DEPTH-1) then + if (uuid_cnt = F_GOAL_ID_MAX_DEPTH-1) then decode_stage_next <= GET_SEQ_LENGTH; uuid_cnt_next <= 0; -- Post-reset else @@ -379,8 +379,8 @@ begin -- DONE stage_next <= SKIP_PAYLOAD; else - if (tmp_length > SEQ_MAX_DEPTH) then - seq_len_latch_next <= to_unsigned(SEQ_MAX_DEPTH, seq_len_latch_next'length); + if (tmp_length > F_SEQ_MAX_DEPTH) then + seq_len_latch_next <= to_unsigned(F_SEQ_MAX_DEPTH, seq_len_latch_next'length); else seq_len_latch_next <= resize(tmp_length, seq_len_latch_next'length); end if; @@ -394,7 +394,7 @@ begin target_align_next <= ALIGN_4; stage_next <= ALIGN_STREAM; else - seq_mem_addr <= std_logic_vector(to_unsigned(seq_cnt,SEQ_ADDR_WIDTH)); + seq_mem_addr <= std_logic_vector(to_unsigned(seq_cnt,F_SEQ_ADDR_WIDTH)); seq_mem_data_in <= endian_swap(endian_flag, data_in_latch); seq_mem_valid_in <= '1'; -- Memory Operation Guard diff --git a/src/ros2/example_interfaces/Fibonacci_ros_action_goal_srv_client.vhd b/src/ros2/example_interfaces/Fibonacci_ros_action_goal_srv_client.vhd new file mode 100644 index 0000000..3a166ba --- /dev/null +++ b/src/ros2/example_interfaces/Fibonacci_ros_action_goal_srv_client.vhd @@ -0,0 +1,760 @@ +-- altera vhdl_input_version vhdl_2008 +-- XXX: QSYS Fix (https://www.intel.com/content/www/us/en/support/programmable/articles/000079458.html) + +library ieee; +use ieee.std_logic_1164.all; +use ieee.numeric_std.all; + +use work.rtps_package.all; +use work.rtps_config_package.all; +use work.ros_package.all; +use work.Fibonacci_package.all; + +entity Fibonacci_ros_action_goal_srv_client is + generic ( + LITTLE_ENDIAN : std_logic := '0' + ); + port ( + -- SYSTEM + clk : in std_logic; + reset : in std_logic; + -- FROM DDS READER + start_r : out std_logic; + ack_r : in std_logic; + opcode_r : out DDS_READER_OPCODE_TYPE; + instance_state_r : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0); + view_state_r : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0); + sample_state_r : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0); + instance_handle_r : out INSTANCE_HANDLE_TYPE; + max_samples_r : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0); + get_data_r : out std_logic; + done_r : in std_logic; + return_code_r : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + valid_in_r : in std_logic; + ready_in_r : out std_logic; + data_in_r : in std_logic_vector(WORD_WIDTH-1 downto 0); + last_word_in_r : in std_logic; + -- Sample Info + si_sample_state_r : in std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0); + si_view_state_r : in std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0); + si_instance_state_r : in std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0); + si_source_timestamp_r : in TIME_TYPE; + si_instance_handle_r : in INSTANCE_HANDLE_TYPE; + si_publication_handle_r : in INSTANCE_HANDLE_TYPE; + si_disposed_generation_count_r : in std_logic_vector(DISPOSED_GENERATION_COUNT_WIDTH-1 downto 0); + si_no_writers_generation_count_r : in std_logic_vector(NO_WRITERS_GENERATION_COUNT_WIDTH-1 downto 0); + si_sample_rank_r : in std_logic_vector(SAMPLE_RANK_WIDTH-1 downto 0); + si_generation_rank_r : in std_logic_vector(GENERATION_RANK_WIDTH-1 downto 0); + si_absolute_generation_rank_r : in std_logic_vector(ABSOLUTE_GENERATION_COUNT_WIDTH-1 downto 0); + si_valid_data_r : in std_logic; + si_valid_r : in std_logic; + si_ack_r : out std_logic; + eoc_r : in std_logic; + status_r : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + -- FROM DDS WRITER + start_w : out std_logic; + ack_w : in std_logic; + opcode_w : out DDS_WRITER_OPCODE_TYPE; + instance_handle_out_w : out INSTANCE_HANDLE_TYPE; + source_ts_w : out TIME_TYPE; + max_wait_w : out DURATION_TYPE; + done_w : in std_logic; + return_code_w : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + instance_handle_in_w : in INSTANCE_HANDLE_TYPE; + valid_out_w : out std_logic; + ready_out_w : in std_logic; + data_out_w : out std_logic_vector(WORD_WIDTH-1 downto 0); + last_word_out_w : out std_logic; + valid_in_w : in std_logic; + ready_in_w : out std_logic; + data_in_w : in std_logic_vector(WORD_WIDTH-1 downto 0); + last_word_in_w : in std_logic; + status_w : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + + -- TO USER + start_user : in std_logic; + ack_user : out std_logic; + opcode_user : in ROS_SERVICE_OPCODE_TYPE; + service_info_user : out SERVICE_INFO_TYPE; + sequence_id_user : out std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0); + data_available_user : out std_logic; + taken_user : out std_logic; + -- REQUEST + goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0); + -- ###GENERATED START### + order : in std_logic_vector(CDR_LONG_WIDTH-1 downto 0); + -- ###GENERATED END### + -- RESPONSE + accepted : out std_logic; + stamp : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + done_user : out std_logic; + return_code_user : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) + ); +end entity; + +architecture arch of Fibonacci_ros_action_goal_srv_client is + + --*****TYPE DECLARATION***** + -- FSM states. Explained below in detail + type STAGE_TYPE is (IDLE,RETURN_ROS,INITIATE_READ,WAIT_FOR_READER,WAIT_FOR_WRITER,WAIT_FOR_DATA,GET_PAYLOAD_HEADER,FETCH,ALIGN_IN_STREAM,SKIP_PAYLOAD,DECODE_PAYLOAD,INITIATE_WRITE,WRITE_PAYLOAD_HEADER,PUSH,ALIGN_OUT_STREAM,ENCODE_PAYLOAD); + -- ###GENERATED START### + type ENCODE_STAGE_TYPE is (WRITE_RID_WGUID,WRITE_RID_SN,WRITE_RQ_GOAL_ID,WRITE_RQ_ORDER); + type DECODE_STAGE_TYPE is (GET_RID_WGUID,GET_RID_SN,GET_OPTIONAL_HEADER,GET_RR_ACCEPTED,GET_RR_STAMP); + -- ###GENERATED END### + + -- *MAIN PROCESS* + signal stage, stage_next : STAGE_TYPE; + signal cnt, cnt_next : natural range 0 to 5; + signal endian_flag, endian_flag_next : std_logic; + signal last_word_in_latch, last_word_in_latch_next : std_logic; + signal decode_error_latch, decode_error_latch_next : std_logic; + signal dw_latch, dw_latch_next : std_logic_vector(CDR_LONG_LONG_WIDTH-1 downto 0); + signal align_offset, align_offset_next : unsigned(MAX_ALIGN_OFFSET_WIDTH-1 downto 0); + signal align_op, align_op_next : std_logic; + signal target_align, target_align_next : ALIGN_TYPE; + signal data_in_latch, data_in_latch_next : std_logic_vector(WORD_WIDTH-1 downto 0); + signal optional, optional_next : std_logic; + signal data_out_latch, data_out_latch_next : std_logic_vector(WORD_WIDTH-1 downto 0); + signal abort_mem : std_logic; + signal ready_in_r_sig : std_logic; + signal taken_sig, taken_sig_next : std_logic; + signal service_info_sig, service_info_sig_next : SERVICE_INFO_TYPE; + signal sequence_id_sig, sequence_id_sig_next : unsigned(ROS_SEQUENCE_ID_WIDTH-1 downto 0); + signal finalize_payload, finalize_payload_next : std_logic; + signal encode_stage, encode_stage_next : ENCODE_STAGE_TYPE; + signal decode_stage, decode_stage_next : DECODE_STAGE_TYPE; + signal return_stage, return_stage_next : DECODE_STAGE_TYPE; + signal return_code_latch, return_code_latch_next : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + signal data_available_sig, data_available_sig_next : std_logic; + signal uuid_cnt, uuid_cnt_next : natural range 0 to G_RQ_GOAL_ID_MAX_DEPTH-1; + signal accepted_latch, accepted_latch_next : std_logic; + signal stamp_latch, stamp_latch_next : std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + -- ###GENERATED START### + -- ###GENERATED END### + + --*****ALIAS DECLARATION***** + alias representation_id : std_logic_vector(PAYLOAD_REPRESENTATION_ID_WIDTH-1 downto 0) is data_in_r(WORD_WIDTH-1 downto WORD_WIDTH-PAYLOAD_REPRESENTATION_ID_WIDTH); + alias representation_options : std_logic_vector(PAYLOAD_REPRESENTATION_ID_WIDTH-1 downto 0) is data_in_r(PAYLOAD_REPRESENTATION_OPTIONS_WIDTH-1 downto 0); + alias parameter_id : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) is data_in_latch(WORD_WIDTH-1 downto WORD_WIDTH-PARAMETER_ID_WIDTH); + alias parameter_length : std_logic_vector(PARAMETER_LENGTH_WIDTH-1 downto 0) is data_in_latch(PARAMETER_LENGTH_WIDTH-1 downto 0); + + +begin + + -- ###GENERATED START### + -- MEMORY INSTANTIATIONS + -- ###GENERATED END### + + instance_state_r <= ANY_INSTANCE_STATE; + view_state_r <= ANY_VIEW_STATE; + sample_state_r <= ANY_SAMPLE_STATE; + instance_handle_r <= HANDLE_NIL; + max_samples_r <= (others => '0'); + instance_handle_out_w <= HANDLE_NIL; + source_ts_w <= TIME_INVALID; + max_wait_w <= DURATION_ZERO; + ready_in_w <= '0'; -- DDS Writer Input is unused + taken_user <= taken_sig; + ready_in_r <= ready_in_r_sig; + service_info_user <= service_info_sig; + sequence_id_user <= std_logic_vector(sequence_id_sig); + data_available_user <= data_available_sig; + + -- ###GENERATED START### + accepted <= accepted_latch; + stamp <= stamp_latch; + -- ###GENERATED END### + + main_prc : process (all) + variable tmp_length : unsigned(WORD_WIDTH-1 downto 0); + begin + -- DEFAULT + stage_next <= stage; + encode_stage_next <= encode_stage; + decode_stage_next <= decode_stage; + return_stage_next <= return_stage; + cnt_next <= cnt; + endian_flag_next <= endian_flag; + last_word_in_latch_next <= last_word_in_latch; + decode_error_latch_next <= decode_error_latch; + align_offset_next <= align_offset; + target_align_next <= target_align; + data_out_latch_next <= data_out_latch; + finalize_payload_next <= finalize_payload; + optional_next <= optional; + taken_sig_next <= taken_sig; + data_in_latch_next <= data_in_latch; + align_op_next <= align_op; + return_code_latch_next <= return_code_latch; + service_info_sig_next <= service_info_sig; + dw_latch_next <= dw_latch; + sequence_id_sig_next <= sequence_id_sig; + data_available_sig_next <= data_available_sig; + ready_in_r_sig <= '0'; + abort_mem <= '0'; + ack_user <= '0'; + si_ack_r <= '0'; + get_data_r <= '0'; + start_r <= '0'; + opcode_r <= NOP; + start_w <= '0'; + opcode_w <= NOP; + valid_out_w <= '0'; + last_word_out_w <= '0'; + done_user <= '0'; + return_code_user <= ROS_RET_OK; + data_out_w <= (others => '0'); + uuid_cnt_next <= uuid_cnt; + -- ###GENERATED START### + accepted_latch_next <= accepted_latch; + stamp_latch_next <= stamp_latch; + -- ###GENERATED END### + + -- Last Word Latch Setter + if (last_word_in_r = '1') then + last_word_in_latch_next <= '1'; + end if; + -- Data Available Setter + if (check_mask(status_r, DATA_AVAILABLE_STATUS)) then + data_available_sig_next <= '1'; + end if; + + + case (stage) is + when IDLE => + if (start_user = '1') then + ack_user <= '1'; + case (opcode_user) is + when SEND_REQUEST => + stage_next <= INITIATE_WRITE; + -- Increment Sequence ID + sequence_id_sig_next <= sequence_id_sig + 1; + when TAKE_RESPONSE => + stage_next <= INITIATE_READ; + when others => + return_code_latch_next <= ROS_RET_UNSUPPORTED; + stage_next <= RETURN_ROS; + end case; + -- RESET + taken_sig_next <= '0'; + abort_mem <= '1'; + else + -- ###GENERATED START### + -- MEMORY SIGNAL CONNECTIONS + -- ###GENERATED END### + end if; + when RETURN_ROS => + done_user <= '1'; + return_code_user <= return_code_latch; + + -- DONE + stage_next <= IDLE; + when INITIATE_READ => + start_r <= '1'; + opcode_r <= TAKE_NEXT_SAMPLE; + + if (ack_r = '1') then + stage_next <= WAIT_FOR_READER; + end if; + when WAIT_FOR_READER => + if (done_r = '1') then + case (return_code_r) is + when RETCODE_OK => + stage_next <= WAIT_FOR_DATA; + when RETCODE_NO_DATA => + assert (taken_sig = '0') severity FAILURE; + + -- Data Available Resetter + data_available_sig_next <= '0'; + + return_code_latch_next <= ROS_RET_OK; + stage_next <= RETURN_ROS; + when others => + return_code_latch_next <= ROS_RET_ERROR; + stage_next <= RETURN_ROS; + end case; + end if; + when WAIT_FOR_DATA => + if (si_valid_r = '1') then + si_ack_r <= '1'; + -- Meta Sample + if (si_valid_data_r = '0') then + -- Ignore and read Next Sample + stage_next <= INITIATE_READ; + else + get_data_r <= '1'; + stage_next <= GET_PAYLOAD_HEADER; + + service_info_sig_next.received_timestamp <= TIME_INVALID; + service_info_sig_next.source_timestamp <= si_source_timestamp_r; + end if; + end if; + when GET_PAYLOAD_HEADER => + -- TODO: Latch Offset from Options Field? + + ready_in_r_sig <= '1'; + -- Input Guard + if (valid_in_r = '1') then + case (representation_id) is + when CDR_BE => + endian_flag_next <= '0'; + stage_next <= FETCH; + -- Alignment Reset + align_offset_next <= (others => '0'); + decode_stage_next <= GET_RID_WGUID; + cnt_next <= 0; + -- Initial Fetch + when CDR_LE => + endian_flag_next <= '1'; + stage_next <= FETCH; + -- Alignment Reset + align_offset_next <= (others => '0'); + decode_stage_next <= GET_RID_WGUID; + cnt_next <= 0; + when others => + -- Unknown Payload Encoding + stage_next <= SKIP_PAYLOAD; + decode_error_latch_next <= '1'; + end case; + end if; + when FETCH => + ready_in_r_sig <= '1'; + -- Input Guard + if (valid_in_r = '1') then + data_in_latch_next <= data_in_r; + -- Alignment Operation in progress + if (align_op = '1') then + stage_next <= ALIGN_IN_STREAM; + -- Reset + align_op_next <= '0'; + else + stage_next <= DECODE_PAYLOAD; + end if; + end if; + when ALIGN_IN_STREAM => + -- Target Stream Alignment reached + if (check_align(align_offset, target_align)) then + -- DONE + stage_next <= DECODE_PAYLOAD; + else + align_offset_next <= align_offset + 1; + -- Need to fetch new Input Word + if (align_offset(1 downto 0) = "11") then + align_op_next <= '1'; + stage_next <= FETCH; + end if; + end if; + when DECODE_PAYLOAD => + case (decode_stage) is + -- NOTE: The Cyclone DDS implementation uses a custom request header that is pre-pended to the actual service request/response. + -- It is defined as follows: + -- struct cdds_request_header_t{ + -- uint64_t guid; + -- int64_t seq; + -- }; + -- 'seq' is set by a counter that is incremented on each "send_request". + -- 'guid' is set to the publication handle of the request writer of the service client. + -- Note that the publication handle is useless for the server, since it is only meanigful localy (i.e. only the client can do something with it) + -- Nevertheless the same 'guid' has to be returned to the client. + when GET_RID_WGUID => + -- ALIGN GUARD + if (not check_align(align_offset, ALIGN_8)) then + target_align_next <= ALIGN_8; + stage_next <= ALIGN_IN_STREAM; + else + case (cnt) is + -- Double Word 1/2 + when 0 => + dw_latch_next(CDR_LONG_LONG_WIDTH-1 downto CDR_LONG_LONG_WIDTH/2) <= data_in_latch; + stage_next <= FETCH; + cnt_next <= cnt + 1; + -- Double Word 2/2 + when 1 => + dw_latch_next((CDR_LONG_LONG_WIDTH/2)-1 downto 0) <= data_in_latch; + cnt_next <= cnt + 1; + -- Push Double Word + when 2 => + service_info_sig_next.request_id.writer_guid(0) <= get_sub_vector(endian_swap(endian_flag, dw_latch),0,WORD_WIDTH,TRUE); + service_info_sig_next.request_id.writer_guid(1) <= get_sub_vector(endian_swap(endian_flag, dw_latch),1,WORD_WIDTH,TRUE); + stage_next <= FETCH; + align_offset_next <= align_offset + 8; + decode_stage_next <= GET_RID_SN; + cnt_next <= 0; + when others => + null; + end case; + end if; + when GET_RID_SN => + -- ALIGN GUARD + if (not check_align(align_offset, ALIGN_8)) then + target_align_next <= ALIGN_8; + stage_next <= ALIGN_IN_STREAM; + else + case (cnt) is + -- Double Word 1/2 + when 0 => + dw_latch_next(CDR_LONG_LONG_WIDTH-1 downto CDR_LONG_LONG_WIDTH/2) <= data_in_latch; + stage_next <= FETCH; + cnt_next <= cnt + 1; + -- Double Word 2/2 + when 1 => + dw_latch_next((CDR_LONG_LONG_WIDTH/2)-1 downto 0) <= data_in_latch; + cnt_next <= cnt + 1; + -- Push Double Word + when 2 => + service_info_sig_next.request_id.sequence_number <= to_double_word(unsigned(endian_swap(endian_flag, dw_latch))); + stage_next <= FETCH; + align_offset_next <= align_offset + 8; + decode_stage_next <= GET_RR_ACCEPTED; + when others => + null; + end case; + end if; + when GET_RR_ACCEPTED => + -- ALIGN GUARD + if (not check_align(align_offset, ALIGN_1)) then + target_align_next <= ALIGN_1; + stage_next <= ALIGN_IN_STREAM; + else + accepted_latch_next <= get_sub_vector(data_in_latch, to_integer(align_offset(1 downto 0)), CDR_BOOLEAN_WIDTH, TRUE)(0); + align_offset_next <= align_offset + 1; + decode_stage_next <= GET_RR_STAMP; + cnt_next <= 0; + + -- Need to fetch next Word + if(align_offset(1 downto 0) = "11") then + stage_next <= FETCH; + end if; + end if; + when GET_RR_STAMP => + -- We intepret the TIME as a double word for efficiency + -- ALIGN GUARD + if (not check_align(align_offset, ALIGN_4)) then + target_align_next <= ALIGN_4; + stage_next <= ALIGN_IN_STREAM; + else + case (cnt) is + -- Double Word 1/2 + when 0 => + dw_latch_next <= write_sub_vector(dw_latch_next, endian_swap(endian_flag, data_in_latch), 0, TRUE); + stage_next <= FETCH; + cnt_next <= cnt + 1; + -- Double Word 2/2 + when 1 => + dw_latch_next <= write_sub_vector(dw_latch_next, endian_swap(endian_flag, data_in_latch), 1, TRUE); + cnt_next <= cnt + 1; + -- Push Double Word + when 2 => + stamp_latch_next <= dw_latch; + stage_next <= FETCH; + align_offset_next <= align_offset + 8; + + -- DONE + stage_next <= SKIP_PAYLOAD; + when others => + null; + end case; + end if; + when GET_OPTIONAL_HEADER => + -- ALIGN GUARD + if (not check_align(align_offset, ALIGN_4)) then + target_align_next <= ALIGN_4; + stage_next <= ALIGN_IN_STREAM; + else + case (cnt) is + -- Optional Member Header + when 0 => + -- Extended Parameter Header + if (endian_swap(endian_flag,parameter_id) = PID_EXTENDED) then + cnt_next <= cnt + 1; + stage_next <= FETCH; + else + stage_next <= FETCH; + decode_stage_next <= return_stage; + cnt_next <= 0; + -- Alignment Reset + align_offset_next <= (others => '0'); + + -- Optional omitted + if(endian_swap(endian_flag,parameter_length) = (parameter_length'reverse_range => '0')) then + optional_next <= '0'; + else + optional_next <= '1'; + end if; + end if; + -- eMemberHeader + when 1 => + -- Ignore Parameter ID + cnt_next <= cnt + 1; + stage_next <= FETCH; + -- Llength + when 2 => + stage_next <= FETCH; + decode_stage_next <= return_stage; + cnt_next <= 0; + -- Alignment Reset + align_offset_next <= (others => '0'); + + -- Optional omitted + if(endian_swap(endian_flag, data_in_r) = (data_in_r'reverse_range => '0')) then + optional_next <= '0'; + else + optional_next <= '1'; + end if; + when others => + null; + end case; + end if; + when others => + null; + end case; + when SKIP_PAYLOAD => + if (last_word_in_latch = '0') then + -- Skip Read + ready_in_r_sig <= '1'; + else + -- Reset + last_word_in_latch_next <= '0'; + + -- If no Decode Error, mark output as valid + if (decode_error_latch = '0') then + taken_sig_next <= '1'; + return_code_latch_next <= ROS_RET_OK; + else + taken_sig_next <= '0'; + return_code_latch_next <= ROS_RET_ERROR; + end if; + + stage_next <= RETURN_ROS; + end if; + when INITIATE_WRITE => + start_w <= '1'; + opcode_w <= WRITE; + + if (ack_w = '1') then + stage_next <= WRITE_PAYLOAD_HEADER; + end if; + when WRITE_PAYLOAD_HEADER => + valid_out_w <= '1'; + if (LITTLE_ENDIAN = '0') then + data_out_w <= CDR_BE & x"0000"; + else + data_out_w <= CDR_LE & x"0000"; + end if; + -- Output Guard + if (ready_out_w = '1') then + stage_next <= ENCODE_PAYLOAD; + -- Reset + align_offset_next <= (others => '0'); + data_out_latch_next <= (others => '0'); + encode_stage_next <= WRITE_RID_WGUID; + cnt_next <= 0; + end if; + when PUSH => + -- Mark Last Word + if (finalize_payload = '1') then + last_word_out_w <= '1'; + end if; + + valid_out_w <= '1'; + data_out_w <= data_out_latch; + -- Output Guard + if (ready_out_w = '1') then + -- NOTE: Ensures all padding is zero. + data_out_latch_next <= (others => '0'); + -- Alignment Operation in process + if (align_op = '1') then + stage_next <= ALIGN_OUT_STREAM; + -- Reset + align_op_next <= '0'; + -- DONE + elsif (finalize_payload = '1') then + finalize_payload_next <= '0'; + stage_next <= WAIT_FOR_WRITER; + else + stage_next <= ENCODE_PAYLOAD; + end if; + end if; + when ALIGN_OUT_STREAM => + -- Target Stream Alignment reached + if (check_align(align_offset, target_align)) then + -- DONE + stage_next <= ENCODE_PAYLOAD; + else + align_offset_next <= align_offset + 1; + -- Need to push Word + if (align_offset(1 downto 0) = "11") then + align_op_next <= '1'; + stage_next <= PUSH; + end if; + end if; + when ENCODE_PAYLOAD => + case (encode_stage) is + when WRITE_RID_WGUID => + -- ALIGN GUARD + if (not check_align(align_offset, ALIGN_8)) then + target_align_next <= ALIGN_8; + stage_next <= ALIGN_OUT_STREAM; + else + case (cnt) is + when 0 => + data_out_latch_next <= (others => '0'); + stage_next <= PUSH; + cnt_next <= cnt + 1; + when 1 => + data_out_latch_next <= (others => '0'); + stage_next <= PUSH; + align_offset_next <= align_offset + 8; + encode_stage_next <= WRITE_RID_SN; + cnt_next <= 0; + when others => + end case; + end if; + when WRITE_RID_SN => + -- ALIGN GUARD + if (not check_align(align_offset, ALIGN_8)) then + target_align_next <= ALIGN_8; + stage_next <= ALIGN_OUT_STREAM; + else + case (cnt) is + when 0 => + data_out_latch_next <= get_sub_vector(endian_swap(LITTLE_ENDIAN, std_logic_vector(sequence_id_sig)), 0, WORD_WIDTH, TRUE); + stage_next <= PUSH; + cnt_next <= cnt + 1; + when 1 => + data_out_latch_next <= get_sub_vector(endian_swap(LITTLE_ENDIAN, std_logic_vector(sequence_id_sig)), 1, WORD_WIDTH, TRUE); + stage_next <= PUSH; + align_offset_next <= align_offset + 8; + encode_stage_next <= WRITE_RQ_GOAL_ID; + when others => + end case; + end if; + when WRITE_RQ_GOAL_ID => + -- Special Encoding for effieciency (Prevent having to define memory for UUID) + -- ALIGN GUARD + if (not check_align(align_offset, ALIGN_1)) then + target_align_next <= ALIGN_1; + stage_next <= ALIGN_OUT_STREAM; + else + data_out_latch_next <= write_sub_vector(data_out_latch_next, get_sub_vector(goal_id,uuid_cnt,CDR_INT8_WIDTH,TRUE), to_integer(align_offset(1 downto 0)), TRUE); + align_offset_next <= align_offset + 1; + + if (uuid_cnt = G_RQ_GOAL_ID_MAX_DEPTH-1) then + -- ###GENERATED START### + encode_stage_next <= WRITE_RQ_ORDER; + -- ###GENERATED END### + uuid_cnt_next <= 0; -- Post-Reset + else + uuid_cnt_next <= uuid_cnt + 1; + end if; + + -- Need to fetch next Word + if(align_offset(1 downto 0) = "11") then + stage_next <= PUSH; + end if; + end if; + -- ###GENERATED START### + when WRITE_RQ_ORDER => + -- ALIGN GUARD + if (not check_align(align_offset, ALIGN_4)) then + target_align_next <= ALIGN_4; + stage_next <= ALIGN_OUT_STREAM; + else + data_out_latch_next <= endian_swap(LITTLE_ENDIAN,order); + align_offset_next <= align_offset + 4; + stage_next <= PUSH; + + -- DONE + stage_next <= PUSH; + finalize_payload_next <= '1'; + end if; + -- ###GENERATED END### + when others => + null; + end case; + when WAIT_FOR_WRITER => + if (done_w = '1') then + case (return_code_w) is + when RETCODE_OK => + return_code_latch_next <= ROS_RET_OK; + stage_next <= RETURN_ROS; + when others => + return_code_latch_next <= ROS_RET_ERROR; + stage_next <= RETURN_ROS; + end case; + end if; + when others => + null; + end case; + + -- OVERREAD GUARD + -- Attempted read on empty input + if (last_word_in_latch = '1' and last_word_in_r = '0' and ready_in_r_sig = '1') then + stage_next <= SKIP_PAYLOAD; + decode_error_latch_next <= '1'; + end if; + + end process; + + sync_prc : process(clk) + begin + if rising_edge(clk) then + if (reset = '1') then + stage <= IDLE; + encode_stage <= WRITE_RID_WGUID; + decode_stage <= GET_RID_WGUID; + return_stage <= GET_RID_WGUID; + target_align <= ALIGN_1; + cnt <= 0; + endian_flag <= '0'; + last_word_in_latch <= '0'; + decode_error_latch <= '0'; + optional <= '0'; + taken_sig <= '0'; + align_op <= '0'; + finalize_payload <= '0'; + data_available_sig <= '0'; + align_offset <= (others => '0'); + data_in_latch <= (others => '0'); + data_out_latch <= (others => '0'); + dw_latch <= (others => '0'); + sequence_id_sig <= (others => '0'); + return_code_latch <= ROS_RET_OK; + service_info_sig <= EMPTY_SERVICE_INFO; + uuid_cnt <= 0; + -- ###GENERATED START### + accepted_latch <= '0'; + stamp_latch <= (others => '0'); + -- ###GENERATED END### + else + stage <= stage_next; + encode_stage <= encode_stage_next; + decode_stage <= decode_stage_next; + return_stage <= return_stage_next; + target_align <= target_align_next; + cnt <= cnt_next; + endian_flag <= endian_flag_next; + last_word_in_latch <= last_word_in_latch_next; + decode_error_latch <= decode_error_latch_next; + optional <= optional_next; + taken_sig <= taken_sig_next; + align_op <= align_op_next; + finalize_payload <= finalize_payload_next; + data_available_sig <= data_available_sig_next; + align_offset <= align_offset_next; + data_in_latch <= data_in_latch_next; + data_out_latch <= data_out_latch_next; + dw_latch <= dw_latch_next; + sequence_id_sig <= sequence_id_sig_next; + return_code_latch <= return_code_latch_next; + service_info_sig <= service_info_sig_next; + uuid_cnt <= uuid_cnt_next; + -- ###GENERATED START### + accepted_latch <= accepted_latch_next; + stamp_latch <= stamp_latch_next; + -- ###GENERATED END### + end if; + end if; + end process; + +end architecture; \ No newline at end of file diff --git a/src/ros2/example_interfaces/Fibonacci_ros_action_goal_srv_server.vhd b/src/ros2/example_interfaces/Fibonacci_ros_action_goal_srv_server.vhd new file mode 100644 index 0000000..2244dd3 --- /dev/null +++ b/src/ros2/example_interfaces/Fibonacci_ros_action_goal_srv_server.vhd @@ -0,0 +1,751 @@ +-- altera vhdl_input_version vhdl_2008 +-- XXX: QSYS Fix (https://www.intel.com/content/www/us/en/support/programmable/articles/000079458.html) + +library ieee; +use ieee.std_logic_1164.all; +use ieee.numeric_std.all; + +use work.rtps_package.all; +use work.rtps_config_package.all; +use work.ros_package.all; +use work.Fibonacci_package.all; + + +entity Fibonacci_ros_action_goal_srv_server is + generic ( + LITTLE_ENDIAN : std_logic := '0' + ); + port ( + -- SYSTEM + clk : in std_logic; + reset : in std_logic; + -- FROM DDS READER + start_r : out std_logic; + ack_r : in std_logic; + opcode_r : out DDS_READER_OPCODE_TYPE; + instance_state_r : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0); + view_state_r : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0); + sample_state_r : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0); + instance_handle_r : out INSTANCE_HANDLE_TYPE; + max_samples_r : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0); + get_data_r : out std_logic; + done_r : in std_logic; + return_code_r : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + valid_in_r : in std_logic; + ready_in_r : out std_logic; + data_in_r : in std_logic_vector(WORD_WIDTH-1 downto 0); + last_word_in_r : in std_logic; + -- Sample Info + si_sample_state_r : in std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0); + si_view_state_r : in std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0); + si_instance_state_r : in std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0); + si_source_timestamp_r : in TIME_TYPE; + si_instance_handle_r : in INSTANCE_HANDLE_TYPE; + si_publication_handle_r : in INSTANCE_HANDLE_TYPE; + si_disposed_generation_count_r : in std_logic_vector(DISPOSED_GENERATION_COUNT_WIDTH-1 downto 0); + si_no_writers_generation_count_r : in std_logic_vector(NO_WRITERS_GENERATION_COUNT_WIDTH-1 downto 0); + si_sample_rank_r : in std_logic_vector(SAMPLE_RANK_WIDTH-1 downto 0); + si_generation_rank_r : in std_logic_vector(GENERATION_RANK_WIDTH-1 downto 0); + si_absolute_generation_rank_r : in std_logic_vector(ABSOLUTE_GENERATION_COUNT_WIDTH-1 downto 0); + si_valid_data_r : in std_logic; + si_valid_r : in std_logic; + si_ack_r : out std_logic; + eoc_r : in std_logic; + status_r : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + -- FROM DDS WRITER + start_w : out std_logic; + ack_w : in std_logic; + opcode_w : out DDS_WRITER_OPCODE_TYPE; + instance_handle_out_w : out INSTANCE_HANDLE_TYPE; + source_ts_w : out TIME_TYPE; + max_wait_w : out DURATION_TYPE; + done_w : in std_logic; + return_code_w : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + instance_handle_in_w : in INSTANCE_HANDLE_TYPE; + valid_out_w : out std_logic; + ready_out_w : in std_logic; + data_out_w : out std_logic_vector(WORD_WIDTH-1 downto 0); + last_word_out_w : out std_logic; + valid_in_w : in std_logic; + ready_in_w : out std_logic; + data_in_w : in std_logic_vector(WORD_WIDTH-1 downto 0); + last_word_in_w : in std_logic; + status_w : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + + -- TO USER + start_user : in std_logic; + ack_user : out std_logic; + opcode_user : in ROS_SERVICE_OPCODE_TYPE; + service_info_user : out SERVICE_INFO_TYPE; + request_id_user : in REQUEST_ID_TYPE; + data_available_user : out std_logic; + taken_user : out std_logic; + -- REQUEST + goal_id : out std_logic_vector(UUID_WIDTH-1 downto 0); + -- ###GENERATED START### + order : out std_logic_vector(CDR_LONG_WIDTH-1 downto 0); + -- ###GENERATED END### + -- RESPONSE + accepted : in std_logic; + stamp : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + + done_user : out std_logic; + return_code_user : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) + ); +end entity; + +architecture arch of Fibonacci_ros_action_goal_srv_server is + + --*****TYPE DECLARATION***** + -- FSM states. Explained below in detail + type STAGE_TYPE is (IDLE,RETURN_ROS,INITIATE_READ,WAIT_FOR_READER,WAIT_FOR_WRITER,WAIT_FOR_DATA,GET_PAYLOAD_HEADER,FETCH,ALIGN_IN_STREAM,SKIP_PAYLOAD,DECODE_PAYLOAD,INITIATE_WRITE,WRITE_PAYLOAD_HEADER,PUSH,ALIGN_OUT_STREAM,ENCODE_PAYLOAD); + -- ###GENERATED START### + type ENCODE_STAGE_TYPE is (WRITE_RID_WGUID,WRITE_RID_SN,WRITE_RR_ACCEPTED,WRITE_RR_STAMP); + type DECODE_STAGE_TYPE is (GET_RID_WGUID,GET_RID_SN,GET_OPTIONAL_HEADER,GET_RQ_GOAL_ID,GET_RQ_ORDER); + -- ###GENERATED END### + + -- *MAIN PROCESS* + signal stage, stage_next : STAGE_TYPE; + signal cnt, cnt_next : natural range 0 to 5; + signal endian_flag, endian_flag_next : std_logic; + signal last_word_in_latch, last_word_in_latch_next : std_logic; + signal decode_error_latch, decode_error_latch_next : std_logic; + signal dw_latch, dw_latch_next : std_logic_vector(CDR_LONG_LONG_WIDTH-1 downto 0); + signal align_offset, align_offset_next : unsigned(MAX_ALIGN_OFFSET_WIDTH-1 downto 0); + signal align_op, align_op_next : std_logic; + signal target_align, target_align_next : ALIGN_TYPE; + signal data_in_latch, data_in_latch_next : std_logic_vector(WORD_WIDTH-1 downto 0); + signal optional, optional_next : std_logic; + signal data_out_latch, data_out_latch_next : std_logic_vector(WORD_WIDTH-1 downto 0); + signal abort_mem : std_logic; + signal ready_in_r_sig : std_logic; + signal taken_sig, taken_sig_next : std_logic; + signal service_info_sig, service_info_sig_next : SERVICE_INFO_TYPE; + signal finalize_payload, finalize_payload_next : std_logic; + signal encode_stage, encode_stage_next : ENCODE_STAGE_TYPE; + signal decode_stage, decode_stage_next : DECODE_STAGE_TYPE; + signal return_stage, return_stage_next : DECODE_STAGE_TYPE; + signal return_code_latch, return_code_latch_next : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + signal data_available_sig, data_available_sig_next : std_logic; + signal uuid_cnt, uuid_cnt_next : natural range 0 to G_RQ_GOAL_ID_MAX_DEPTH-1; + signal goal_id_latch, goal_id_latch_next : std_logic_vector(UUID_WIDTH-1 downto 0); + -- ###GENERATED START### + signal order_latch, order_latch_next : std_logic_vector(CDR_LONG_WIDTH-1 downto 0); + -- ###GENERATED END### + + --*****ALIAS DECLARATION***** + alias representation_id : std_logic_vector(PAYLOAD_REPRESENTATION_ID_WIDTH-1 downto 0) is data_in_r(WORD_WIDTH-1 downto WORD_WIDTH-PAYLOAD_REPRESENTATION_ID_WIDTH); + alias representation_options : std_logic_vector(PAYLOAD_REPRESENTATION_ID_WIDTH-1 downto 0) is data_in_r(PAYLOAD_REPRESENTATION_OPTIONS_WIDTH-1 downto 0); + alias parameter_id : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) is data_in_latch(WORD_WIDTH-1 downto WORD_WIDTH-PARAMETER_ID_WIDTH); + alias parameter_length : std_logic_vector(PARAMETER_LENGTH_WIDTH-1 downto 0) is data_in_latch(PARAMETER_LENGTH_WIDTH-1 downto 0); + + +begin + + -- ###GENERATED START### + -- MEMORY INSTANTIATIONS + -- ###GENERATED END### + + instance_state_r <= ANY_INSTANCE_STATE; + view_state_r <= ANY_VIEW_STATE; + sample_state_r <= ANY_SAMPLE_STATE; + instance_handle_r <= HANDLE_NIL; + max_samples_r <= (others => '0'); + instance_handle_out_w <= HANDLE_NIL; + source_ts_w <= TIME_INVALID; + max_wait_w <= DURATION_ZERO; + ready_in_w <= '0'; -- DDS Writer Input is unused + taken_user <= taken_sig; + ready_in_r <= ready_in_r_sig; + service_info_user <= service_info_sig; + data_available_user <= data_available_sig; + goal_id <= goal_id_latch; + -- ###GENERATED START### + order <= order_latch; + -- ###GENERATED END### + + main_prc : process (all) + variable tmp_length : unsigned(WORD_WIDTH-1 downto 0); + begin + -- DEFAULT + stage_next <= stage; + encode_stage_next <= encode_stage; + decode_stage_next <= decode_stage; + return_stage_next <= return_stage; + cnt_next <= cnt; + endian_flag_next <= endian_flag; + last_word_in_latch_next <= last_word_in_latch; + decode_error_latch_next <= decode_error_latch; + align_offset_next <= align_offset; + target_align_next <= target_align; + data_out_latch_next <= data_out_latch; + finalize_payload_next <= finalize_payload; + optional_next <= optional; + taken_sig_next <= taken_sig; + data_in_latch_next <= data_in_latch; + align_op_next <= align_op; + return_code_latch_next <= return_code_latch; + service_info_sig_next <= service_info_sig; + dw_latch_next <= dw_latch; + data_available_sig_next <= data_available_sig; + ready_in_r_sig <= '0'; + abort_mem <= '0'; + ack_user <= '0'; + si_ack_r <= '0'; + get_data_r <= '0'; + start_r <= '0'; + opcode_r <= NOP; + start_w <= '0'; + opcode_w <= NOP; + valid_out_w <= '0'; + last_word_out_w <= '0'; + done_user <= '0'; + return_code_user <= ROS_RET_OK; + data_out_w <= (others => '0'); + uuid_cnt_next <= uuid_cnt; + goal_id_latch_next <= goal_id_latch; + -- ###GENERATED START### + order_latch_next <= order_latch; + -- ###GENERATED END### + + -- Last Word Latch Setter + if (last_word_in_r = '1') then + last_word_in_latch_next <= '1'; + end if; + -- Data Available Setter + if (check_mask(status_r, DATA_AVAILABLE_STATUS)) then + data_available_sig_next <= '1'; + end if; + + + case (stage) is + when IDLE => + if (start_user = '1') then + ack_user <= '1'; + case (opcode_user) is + when TAKE_REQUEST => + stage_next <= INITIATE_READ; + when SEND_RESPONSE => + stage_next <= INITIATE_WRITE; + when others => + return_code_latch_next <= ROS_RET_UNSUPPORTED; + stage_next <= RETURN_ROS; + end case; + -- RESET + taken_sig_next <= '0'; + abort_mem <= '1'; + else + -- ###GENERATED START### + -- MEMORY SIGNAL CONNECTIONS + -- ###GENERATED END### + end if; + when RETURN_ROS => + done_user <= '1'; + return_code_user <= return_code_latch; + + -- DONE + stage_next <= IDLE; + when INITIATE_READ => + start_r <= '1'; + opcode_r <= TAKE_NEXT_SAMPLE; + + if (ack_r = '1') then + stage_next <= WAIT_FOR_READER; + end if; + when WAIT_FOR_READER => + if (done_r = '1') then + case (return_code_r) is + when RETCODE_OK => + stage_next <= WAIT_FOR_DATA; + when RETCODE_NO_DATA => + assert (taken_sig = '0') severity FAILURE; + + -- Data Available Resetter + data_available_sig_next <= '0'; + + return_code_latch_next <= ROS_RET_OK; + stage_next <= RETURN_ROS; + when others => + return_code_latch_next <= ROS_RET_ERROR; + stage_next <= RETURN_ROS; + end case; + end if; + when WAIT_FOR_DATA => + if (si_valid_r = '1') then + si_ack_r <= '1'; + -- Meta Sample + if (si_valid_data_r = '0') then + -- Ignore and read Next Sample + stage_next <= INITIATE_READ; + else + get_data_r <= '1'; + stage_next <= GET_PAYLOAD_HEADER; + + service_info_sig_next.received_timestamp <= TIME_INVALID; + service_info_sig_next.source_timestamp <= si_source_timestamp_r; + end if; + end if; + when GET_PAYLOAD_HEADER => + -- TODO: Latch Offset from Options Field? + + ready_in_r_sig <= '1'; + -- Input Guard + if (valid_in_r = '1') then + case (representation_id) is + when CDR_BE => + endian_flag_next <= '0'; + stage_next <= FETCH; + -- Alignment Reset + align_offset_next <= (others => '0'); + decode_stage_next <= GET_RID_WGUID; + cnt_next <= 0; + -- Initial Fetch + when CDR_LE => + endian_flag_next <= '1'; + stage_next <= FETCH; + -- Alignment Reset + align_offset_next <= (others => '0'); + decode_stage_next <= GET_RID_WGUID; + cnt_next <= 0; + when others => + -- Unknown Payload Encoding + stage_next <= SKIP_PAYLOAD; + decode_error_latch_next <= '1'; + end case; + end if; + when FETCH => + ready_in_r_sig <= '1'; + -- Input Guard + if (valid_in_r = '1') then + data_in_latch_next <= data_in_r; + -- Alignment Operation in progress + if (align_op = '1') then + stage_next <= ALIGN_IN_STREAM; + -- Reset + align_op_next <= '0'; + else + stage_next <= DECODE_PAYLOAD; + end if; + end if; + when ALIGN_IN_STREAM => + -- Target Stream Alignment reached + if (check_align(align_offset, target_align)) then + -- DONE + stage_next <= DECODE_PAYLOAD; + else + align_offset_next <= align_offset + 1; + -- Need to fetch new Input Word + if (align_offset(1 downto 0) = "11") then + align_op_next <= '1'; + stage_next <= FETCH; + end if; + end if; + when DECODE_PAYLOAD => + case (decode_stage) is + -- NOTE: The Cyclone DDS implementation uses a custom request header that is pre-pended to the actual service request/response. + -- It is defined as follows: + -- struct cdds_request_header_t{ + -- uint64_t guid; + -- int64_t seq; + -- }; + -- 'seq' is set by a counter that is incremented on each "send_request". + -- 'guid' is set to the publication handle of the request writer of the service client. + -- Note that the publication handle is useless for the server, since it is only meanigful localy (i.e. only the client can do something with it) + -- Nevertheless the same 'guid' has to be returned to the client. + when GET_RID_WGUID => + -- ALIGN GUARD + if (not check_align(align_offset, ALIGN_8)) then + target_align_next <= ALIGN_8; + stage_next <= ALIGN_IN_STREAM; + else + case (cnt) is + -- Double Word 1/2 + when 0 => + dw_latch_next(CDR_LONG_LONG_WIDTH-1 downto CDR_LONG_LONG_WIDTH/2) <= data_in_latch; + stage_next <= FETCH; + cnt_next <= cnt + 1; + -- Double Word 2/2 + when 1 => + dw_latch_next((CDR_LONG_LONG_WIDTH/2)-1 downto 0) <= data_in_latch; + cnt_next <= cnt + 1; + -- Push Double Word + when 2 => + service_info_sig_next.request_id.writer_guid(0) <= get_sub_vector(endian_swap(endian_flag, dw_latch),0,WORD_WIDTH,TRUE); + service_info_sig_next.request_id.writer_guid(1) <= get_sub_vector(endian_swap(endian_flag, dw_latch),1,WORD_WIDTH,TRUE); + stage_next <= FETCH; + align_offset_next <= align_offset + 8; + decode_stage_next <= GET_RID_SN; + cnt_next <= 0; + when others => + null; + end case; + end if; + when GET_RID_SN => + -- ALIGN GUARD + if (not check_align(align_offset, ALIGN_8)) then + target_align_next <= ALIGN_8; + stage_next <= ALIGN_IN_STREAM; + else + case (cnt) is + -- Double Word 1/2 + when 0 => + dw_latch_next(CDR_LONG_LONG_WIDTH-1 downto CDR_LONG_LONG_WIDTH/2) <= data_in_latch; + stage_next <= FETCH; + cnt_next <= cnt + 1; + -- Double Word 2/2 + when 1 => + dw_latch_next((CDR_LONG_LONG_WIDTH/2)-1 downto 0) <= data_in_latch; + cnt_next <= cnt + 1; + -- Push Double Word + when 2 => + service_info_sig_next.request_id.sequence_number <= to_double_word(unsigned(endian_swap(endian_flag, dw_latch))); + stage_next <= FETCH; + align_offset_next <= align_offset + 8; + + decode_stage_next <= GET_RQ_GOAL_ID; + when others => + null; + end case; + end if; + when GET_RQ_GOAL_ID => + -- Special Decoding for effieciency (Prevent having to define memory for UUID) + -- ALIGN GUARD + if (not check_align(align_offset, ALIGN_1)) then + target_align_next <= ALIGN_1; + stage_next <= ALIGN_IN_STREAM; + else + goal_id_latch_next <= write_sub_vector(goal_id_latch_next, get_sub_vector(data_in_latch, to_integer(align_offset(1 downto 0)), CDR_INT8_WIDTH, TRUE), uuid_cnt, TRUE); + align_offset_next <= align_offset + 1; + + -- Need to fetch next Word + if(align_offset(1 downto 0) = "11") then + stage_next <= FETCH; + end if; + + if (uuid_cnt = G_RQ_GOAL_ID_MAX_DEPTH-1) then + -- ###GENERATED START### + decode_stage_next <= GET_RQ_ORDER; + -- ###GENERATED END### + + uuid_cnt_next <= 0; -- Post-reset + else + uuid_cnt_next <= uuid_cnt + 1; + end if; + end if; + -- ###GENERATED START### + when GET_RQ_ORDER => + -- ALIGN GUARD + if (not check_align(align_offset, ALIGN_4)) then + target_align_next <= ALIGN_4; + stage_next <= ALIGN_IN_STREAM; + else + order_latch_next <= endian_swap(endian_flag, data_in_latch); + stage_next <= FETCH; + align_offset_next <= align_offset + 4; + + -- DONE + stage_next <= SKIP_PAYLOAD; + end if; + -- ###GENERATED END### + when GET_OPTIONAL_HEADER => + -- ALIGN GUARD + if (not check_align(align_offset, ALIGN_4)) then + target_align_next <= ALIGN_4; + stage_next <= ALIGN_IN_STREAM; + else + case (cnt) is + -- Optional Member Header + when 0 => + -- Extended Parameter Header + if (endian_swap(endian_flag,parameter_id) = PID_EXTENDED) then + cnt_next <= cnt + 1; + stage_next <= FETCH; + else + stage_next <= FETCH; + decode_stage_next <= return_stage; + cnt_next <= 0; + -- Alignment Reset + align_offset_next <= (others => '0'); + + -- Optional omitted + if(endian_swap(endian_flag,parameter_length) = (parameter_length'reverse_range => '0')) then + optional_next <= '0'; + else + optional_next <= '1'; + end if; + end if; + -- eMemberHeader + when 1 => + -- Ignore Parameter ID + cnt_next <= cnt + 1; + stage_next <= FETCH; + -- Llength + when 2 => + stage_next <= FETCH; + decode_stage_next <= return_stage; + cnt_next <= 0; + -- Alignment Reset + align_offset_next <= (others => '0'); + + -- Optional omitted + if(endian_swap(endian_flag, data_in_r) = (data_in_r'reverse_range => '0')) then + optional_next <= '0'; + else + optional_next <= '1'; + end if; + when others => + null; + end case; + end if; + when others => + null; + end case; + when SKIP_PAYLOAD => + if (last_word_in_latch = '0') then + -- Skip Read + ready_in_r_sig <= '1'; + else + -- Reset + last_word_in_latch_next <= '0'; + + -- If no Decode Error, mark output as valid + if (decode_error_latch = '0') then + taken_sig_next <= '1'; + return_code_latch_next <= ROS_RET_OK; + else + taken_sig_next <= '0'; + return_code_latch_next <= ROS_RET_ERROR; + end if; + + stage_next <= RETURN_ROS; + end if; + when INITIATE_WRITE => + start_w <= '1'; + opcode_w <= WRITE; + + if (ack_w = '1') then + stage_next <= WRITE_PAYLOAD_HEADER; + end if; + when WRITE_PAYLOAD_HEADER => + valid_out_w <= '1'; + if (LITTLE_ENDIAN = '0') then + data_out_w <= CDR_BE & x"0000"; + else + data_out_w <= CDR_LE & x"0000"; + end if; + -- Output Guard + if (ready_out_w = '1') then + stage_next <= ENCODE_PAYLOAD; + -- Reset + align_offset_next <= (others => '0'); + data_out_latch_next <= (others => '0'); + encode_stage_next <= WRITE_RID_WGUID; + cnt_next <= 0; + end if; + when PUSH => + -- Mark Last Word + if (finalize_payload = '1') then + last_word_out_w <= '1'; + end if; + + valid_out_w <= '1'; + data_out_w <= data_out_latch; + -- Output Guard + if (ready_out_w = '1') then + -- NOTE: Ensures all padding is zero. + data_out_latch_next <= (others => '0'); + -- Alignment Operation in process + if (align_op = '1') then + stage_next <= ALIGN_OUT_STREAM; + -- Reset + align_op_next <= '0'; + -- DONE + elsif (finalize_payload = '1') then + finalize_payload_next <= '0'; + stage_next <= WAIT_FOR_WRITER; + else + stage_next <= ENCODE_PAYLOAD; + end if; + end if; + when ALIGN_OUT_STREAM => + -- Target Stream Alignment reached + if (check_align(align_offset, target_align)) then + -- DONE + stage_next <= ENCODE_PAYLOAD; + else + align_offset_next <= align_offset + 1; + -- Need to push Word + if (align_offset(1 downto 0) = "11") then + align_op_next <= '1'; + stage_next <= PUSH; + end if; + end if; + when ENCODE_PAYLOAD => + case (encode_stage) is + when WRITE_RID_WGUID => + -- ALIGN GUARD + if (not check_align(align_offset, ALIGN_8)) then + target_align_next <= ALIGN_8; + stage_next <= ALIGN_OUT_STREAM; + else + case (cnt) is + when 0 => + data_out_latch_next <= get_sub_vector(endian_swap(LITTLE_ENDIAN, get_sub_vector(std_logic_vector(to_unsigned(request_id_user.writer_guid)), 0, 64, TRUE)), 0, WORD_WIDTH, TRUE); + stage_next <= PUSH; + cnt_next <= cnt + 1; + when 1 => + data_out_latch_next <= get_sub_vector(endian_swap(LITTLE_ENDIAN, get_sub_vector(std_logic_vector(to_unsigned(request_id_user.writer_guid)), 0, 64, TRUE)), 1, WORD_WIDTH, TRUE); + stage_next <= PUSH; + align_offset_next <= align_offset + 8; + encode_stage_next <= WRITE_RID_SN; + cnt_next <= 0; + when others => + end case; + end if; + when WRITE_RID_SN => + -- ALIGN GUARD + if (not check_align(align_offset, ALIGN_8)) then + target_align_next <= ALIGN_8; + stage_next <= ALIGN_OUT_STREAM; + else + case (cnt) is + when 0 => + data_out_latch_next <= get_sub_vector(endian_swap(LITTLE_ENDIAN, std_logic_vector(to_unsigned(request_id_user.sequence_number))), 0, WORD_WIDTH, TRUE); + stage_next <= PUSH; + cnt_next <= cnt + 1; + when 1 => + data_out_latch_next <= get_sub_vector(endian_swap(LITTLE_ENDIAN, std_logic_vector(to_unsigned(request_id_user.sequence_number))), 1, WORD_WIDTH, TRUE); + stage_next <= PUSH; + align_offset_next <= align_offset + 8; + encode_stage_next <= WRITE_RR_ACCEPTED; + when others => + end case; + end if; + when WRITE_RR_ACCEPTED => + -- ALIGN GUARD + if (not check_align(align_offset, ALIGN_1)) then + target_align_next <= ALIGN_1; + stage_next <= ALIGN_OUT_STREAM; + else + data_out_latch_next <= write_sub_vector(data_out_latch, ("0000000" & accepted), to_integer(align_offset(1 downto 0)), TRUE); + align_offset_next <= align_offset + 1; + + encode_stage_next <= WRITE_RR_STAMP; + cnt_next <= 0; + + -- Need to fetch next Word + if(align_offset(1 downto 0) = "11") then + stage_next <= PUSH; + end if; + end if; + when WRITE_RR_STAMP => + -- We intepret the TIME as a double word for efficiency + -- ALIGN GUARD + if (not check_align(align_offset, ALIGN_4)) then + target_align_next <= ALIGN_4; + stage_next <= ALIGN_OUT_STREAM; + else + case (cnt) is + -- WRITE 1/2 + when 0 => + data_out_latch_next <= endian_swap(LITTLE_ENDIAN, get_sub_vector(stamp, 0, WORD_WIDTH, TRUE)); + stage_next <= PUSH; + cnt_next <= cnt + 1; + -- WRITE 2/2 + when 1 => + data_out_latch_next <= endian_swap(LITTLE_ENDIAN, get_sub_vector(stamp, 1, WORD_WIDTH, TRUE)); + align_offset_next <= align_offset + 8; + + -- DONE + stage_next <= PUSH; + finalize_payload_next <= '1'; + when others => + end case; + end if; + when others => + null; + end case; + when WAIT_FOR_WRITER => + if (done_w = '1') then + case (return_code_w) is + when RETCODE_OK => + return_code_latch_next <= ROS_RET_OK; + stage_next <= RETURN_ROS; + when others => + return_code_latch_next <= ROS_RET_ERROR; + stage_next <= RETURN_ROS; + end case; + end if; + when others => + null; + end case; + + -- OVERREAD GUARD + -- Attempted read on empty input + if (last_word_in_latch = '1' and last_word_in_r = '0' and ready_in_r_sig = '1') then + stage_next <= SKIP_PAYLOAD; + decode_error_latch_next <= '1'; + end if; + + end process; + + sync_prc : process(clk) + begin + if rising_edge(clk) then + if (reset = '1') then + stage <= IDLE; + encode_stage <= WRITE_RID_WGUID; + decode_stage <= GET_RID_WGUID; + return_stage <= GET_RID_WGUID; + target_align <= ALIGN_1; + cnt <= 0; + endian_flag <= '0'; + last_word_in_latch <= '0'; + decode_error_latch <= '0'; + optional <= '0'; + taken_sig <= '0'; + align_op <= '0'; + finalize_payload <= '0'; + data_available_sig <= '0'; + align_offset <= (others => '0'); + data_in_latch <= (others => '0'); + data_out_latch <= (others => '0'); + dw_latch <= (others => '0'); + return_code_latch <= ROS_RET_OK; + service_info_sig <= EMPTY_SERVICE_INFO; + uuid_cnt <= 0; + goal_id_latch <= (others => '0'); + -- ###GENERATED START### + order_latch <= (others => '0'); + -- ###GENERATED END### + else + stage <= stage_next; + encode_stage <= encode_stage_next; + decode_stage <= decode_stage_next; + return_stage <= return_stage_next; + target_align <= target_align_next; + cnt <= cnt_next; + endian_flag <= endian_flag_next; + last_word_in_latch <= last_word_in_latch_next; + decode_error_latch <= decode_error_latch_next; + optional <= optional_next; + taken_sig <= taken_sig_next; + align_op <= align_op_next; + finalize_payload <= finalize_payload_next; + data_available_sig <= data_available_sig_next; + align_offset <= align_offset_next; + data_in_latch <= data_in_latch_next; + data_out_latch <= data_out_latch_next; + dw_latch <= dw_latch_next; + return_code_latch <= return_code_latch_next; + service_info_sig <= service_info_sig_next; + uuid_cnt <= uuid_cnt_next; + goal_id_latch <= goal_id_latch_next; + -- ###GENERATED START### + order_latch <= order_latch_next; + -- ###GENERATED END### + end if; + end if; + end process; + +end architecture; \ No newline at end of file