Add ROS CancelGoal Service
This commit is contained in:
parent
c9d4b55d4a
commit
f1cc87c25f
100
sim/L1_CancelGoal_ros_srv_test1.do
Normal file
100
sim/L1_CancelGoal_ros_srv_test1.do
Normal file
@ -0,0 +1,100 @@
|
||||
onerror {resume}
|
||||
radix define ROS_RETCODE {
|
||||
"10#0#" "ROS_RET_OK",
|
||||
"10#1#" "ROS_RET_ERROR",
|
||||
"10#2#" "ROS_RET_TIMEOUT",
|
||||
"10#3#" "ROS_RET_UNSUPPORTED",
|
||||
-default unsigned
|
||||
}
|
||||
quietly WaveActivateNextPane {} 0
|
||||
add wave -noupdate -divider SYSTEM
|
||||
add wave -noupdate /l1_cancelgoal_ros_srv_test1/clk
|
||||
add wave -noupdate /l1_cancelgoal_ros_srv_test1/reset
|
||||
add wave -noupdate -divider CLIENT
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/start_r
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/ack_r
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/get_data_r
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/done_r
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/valid_in_r
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/ready_in_r
|
||||
add wave -noupdate -group CPORTS -radix hexadecimal /l1_cancelgoal_ros_srv_test1/uut_c/data_in_r
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/last_word_in_r
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/si_valid_data_r
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/si_valid_r
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/si_ack_r
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/eoc_r
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/start_w
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/ack_w
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/done_w
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/valid_out_w
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/ready_out_w
|
||||
add wave -noupdate -group CPORTS -radix hexadecimal /l1_cancelgoal_ros_srv_test1/uut_c/data_out_w
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/last_word_out_w
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/start_user
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/ack_user
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/opcode_user
|
||||
add wave -noupdate -group CPORTS -radix hexadecimal /l1_cancelgoal_ros_srv_test1/uut_c/service_info_user
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/taken_user
|
||||
add wave -noupdate -group CPORTS /l1_cancelgoal_ros_srv_test1/uut_c/done_user
|
||||
add wave -noupdate -group CPORTS -radix ROS_RETCODE /l1_cancelgoal_ros_srv_test1/uut_c/return_code_user
|
||||
add wave -noupdate /l1_cancelgoal_ros_srv_test1/uut_c/stage
|
||||
add wave -noupdate /l1_cancelgoal_ros_srv_test1/uut_c/encode_stage
|
||||
add wave -noupdate /l1_cancelgoal_ros_srv_test1/uut_c/decode_stage
|
||||
add wave -noupdate /l1_cancelgoal_ros_srv_test1/uut_c/return_stage
|
||||
add wave -noupdate /l1_cancelgoal_ros_srv_test1/uut_c/cnt
|
||||
add wave -noupdate /l1_cancelgoal_ros_srv_test1/uut_c/decode_error_latch
|
||||
add wave -noupdate /l1_cancelgoal_ros_srv_test1/uut_c/uuid_cnt
|
||||
add wave -noupdate -divider SERVER
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/start_r
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/ack_r
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/get_data_r
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/done_r
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/valid_in_r
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/ready_in_r
|
||||
add wave -noupdate -group SPORTS -radix hexadecimal /l1_cancelgoal_ros_srv_test1/uut_s/data_in_r
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/last_word_in_r
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/si_valid_data_r
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/si_valid_r
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/si_ack_r
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/eoc_r
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/start_w
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/ack_w
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/done_w
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/valid_out_w
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/ready_out_w
|
||||
add wave -noupdate -group SPORTS -radix hexadecimal /l1_cancelgoal_ros_srv_test1/uut_s/data_out_w
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/last_word_out_w
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/start_user
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/ack_user
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/opcode_user
|
||||
add wave -noupdate -group SPORTS -radix hexadecimal /l1_cancelgoal_ros_srv_test1/uut_s/service_info_user
|
||||
add wave -noupdate -group SPORTS -radix hexadecimal /l1_cancelgoal_ros_srv_test1/uut_s/request_id_user
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/taken_user
|
||||
add wave -noupdate -group SPORTS /l1_cancelgoal_ros_srv_test1/uut_s/done_user
|
||||
add wave -noupdate -group SPORTS -radix ROS_RETCODE /l1_cancelgoal_ros_srv_test1/uut_s/return_code_user
|
||||
add wave -noupdate /l1_cancelgoal_ros_srv_test1/uut_s/stage
|
||||
add wave -noupdate /l1_cancelgoal_ros_srv_test1/uut_s/encode_stage
|
||||
add wave -noupdate /l1_cancelgoal_ros_srv_test1/uut_s/decode_stage
|
||||
add wave -noupdate /l1_cancelgoal_ros_srv_test1/uut_s/return_stage
|
||||
add wave -noupdate /l1_cancelgoal_ros_srv_test1/uut_s/cnt
|
||||
add wave -noupdate /l1_cancelgoal_ros_srv_test1/uut_s/decode_error_latch
|
||||
add wave -noupdate /l1_cancelgoal_ros_srv_test1/uut_s/uuid_cnt
|
||||
add wave -noupdate -divider MISC
|
||||
TreeUpdate [SetDefaultTree]
|
||||
WaveRestoreCursors {{Cursor 1} {4025000 ps} 0}
|
||||
quietly wave cursor active 1
|
||||
configure wave -namecolwidth 150
|
||||
configure wave -valuecolwidth 100
|
||||
configure wave -justifyvalue left
|
||||
configure wave -signalnamewidth 1
|
||||
configure wave -snapdistance 10
|
||||
configure wave -datasetprefix 0
|
||||
configure wave -rowmargin 4
|
||||
configure wave -childrowmargin 2
|
||||
configure wave -gridoffset 0
|
||||
configure wave -gridperiod 1
|
||||
configure wave -griddelta 40
|
||||
configure wave -timeline 0
|
||||
configure wave -timelineunits ns
|
||||
update
|
||||
WaveRestoreZoom {5714600 ps} {6738600 ps}
|
||||
488
src/ros2/Tests/Level_1/L1_CancelGoal_ros_srv_test1.vhd
Normal file
488
src/ros2/Tests/Level_1/L1_CancelGoal_ros_srv_test1.vhd
Normal file
@ -0,0 +1,488 @@
|
||||
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;
|
||||
use work.CancelGoal_package.all;
|
||||
|
||||
-- This testbench tests the General Behavour of the AddTwoInts Service.
|
||||
-- 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_CancelGoal_ros_srv_test1 is
|
||||
end entity;
|
||||
|
||||
architecture testbench of L1_CancelGoal_ros_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_info_goal_id_s, goal_info_goal_id_c : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0');
|
||||
signal goal_info_stamp_s, goal_info_stamp_c : std_logic_vector(ROS_TIME_WIDTH-1 downto 0) := (others => '0');
|
||||
signal return_code_s1, return_code_c1 : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0');
|
||||
signal goals_canceling_len_s, goals_canceling_len_c : std_logic_vector(RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0) := (others => '0');
|
||||
signal goals_canceling_addr_s, goals_canceling_addr_c : std_logic_vector(RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0) := (others => '0');
|
||||
signal goals_canceling_ready_s, goals_canceling_ready_c, goals_canceling_ren_s, goals_canceling_ren_c, goals_canceling_wen_s, goals_canceling_valid_s, goals_canceling_valid_c, goals_canceling_ack_s, goals_canceling_ack_c : std_logic := '0';
|
||||
signal goals_canceling_goal_id_r_s, goals_canceling_goal_id_w_s, goals_canceling_goal_id_c : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0');
|
||||
signal goals_canceling_stamp_r_s, goals_canceling_stamp_w_s, goals_canceling_stamp_c : std_logic_vector(ROS_TIME_WIDTH-1 downto 0) := (others => '0');
|
||||
begin
|
||||
|
||||
uut_c : entity work.CancelGoal_ros_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_info_goal_id => goal_info_goal_id_c,
|
||||
goal_info_stamp => goal_info_stamp_c,
|
||||
return_code => return_code_c1,
|
||||
goals_canceling_len => goals_canceling_len_c,
|
||||
goals_canceling_addr => goals_canceling_addr_c,
|
||||
goals_canceling_ready => goals_canceling_ready_c,
|
||||
goals_canceling_ren => goals_canceling_ren_c,
|
||||
goals_canceling_valid => goals_canceling_valid_c,
|
||||
goals_canceling_ack => goals_canceling_ack_c,
|
||||
goals_canceling_goal_id => goals_canceling_goal_id_c,
|
||||
goals_canceling_stamp => goals_canceling_stamp_c,
|
||||
done_user => done_c,--
|
||||
return_code_user => return_code_c --
|
||||
);
|
||||
|
||||
uut_s : entity work.CancelGoal_ros_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_info_goal_id => goal_info_goal_id_s,
|
||||
goal_info_stamp => goal_info_stamp_s,
|
||||
return_code => return_code_s1,
|
||||
goals_canceling_len => goals_canceling_len_s,
|
||||
goals_canceling_addr => goals_canceling_addr_s,
|
||||
goals_canceling_ready => goals_canceling_ready_s,
|
||||
goals_canceling_ren => goals_canceling_ren_s,
|
||||
goals_canceling_wen => goals_canceling_wen_s,
|
||||
goals_canceling_valid => goals_canceling_valid_s,
|
||||
goals_canceling_ack => goals_canceling_ack_s,
|
||||
goals_canceling_goal_id_r => goals_canceling_goal_id_r_s,
|
||||
goals_canceling_goal_id_w => goals_canceling_goal_id_w_s,
|
||||
goals_canceling_stamp_r => goals_canceling_stamp_r_s,
|
||||
goals_canceling_stamp_w => goals_canceling_stamp_w_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 RQ_GOAL_ID, RQ_STAMP, RR_GOAL_ID, RR_STAMP, RR_RETURN_CODE : 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("CancelGoal Service - Level 1 - (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);
|
||||
RQ_GOAL_ID := GetAlertLogID("RQ_GOAL_ID", ALERTLOG_BASE_ID);
|
||||
RQ_STAMP := GetAlertLogID("RQ_STAMP", ALERTLOG_BASE_ID);
|
||||
RR_RETURN_CODE := GetAlertLogID("RR_RETURN_CODE", ALERTLOG_BASE_ID);
|
||||
RR_GOAL_ID := GetAlertLogID("RR_GOAL_ID", ALERTLOG_BASE_ID);
|
||||
RR_STAMP := GetAlertLogID("RR_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_info_goal_id_c <= RV.RandSlv(goal_info_goal_id_c'length);
|
||||
goal_info_stamp_c <= RV.RandSlv(goal_info_stamp_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(RQ_GOAL_ID, goal_info_goal_id_s, goal_info_goal_id_c);
|
||||
AffirmIfEqual(RQ_STAMP, goal_info_stamp_s, goal_info_stamp_c);
|
||||
|
||||
Log("Setting Response", INFO);
|
||||
return_code_s1 <= RV.RandSlv(return_code_s1'length);
|
||||
for i in 0 to 10 loop
|
||||
goals_canceling_len_s <= int(11,goals_canceling_len_s'length);
|
||||
goals_canceling_addr_s <= int(i,goals_canceling_addr_s'length);
|
||||
goals_canceling_goal_id_w_s <= RV.RandSlv(goals_canceling_goal_id_w_s'length);
|
||||
goals_canceling_stamp_w_s <= RV.RandSlv(goals_canceling_stamp_w_s'length);
|
||||
wait_on_sig(goals_canceling_ready_s);
|
||||
goals_canceling_wen_s <= '1';
|
||||
wait for TEST_CLOCK_PERIOD;
|
||||
goals_canceling_wen_s <= '0';
|
||||
end loop;
|
||||
|
||||
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(RR_RETURN_CODE, return_code_c1, return_code_s1);
|
||||
AffirmIfEqual(goals_canceling_len_c, goals_canceling_len_s);
|
||||
for i in 0 to to_integer(unsigned(goals_canceling_len_s))-1 loop
|
||||
goals_canceling_addr_s <= int(i,goals_canceling_addr_s'length);
|
||||
goals_canceling_addr_c <= int(i,goals_canceling_addr_c'length);
|
||||
wait_on_sig(goals_canceling_ready_s);
|
||||
wait_on_sig(goals_canceling_ready_c);
|
||||
goals_canceling_ren_s <= '1';
|
||||
goals_canceling_ren_c <= '1';
|
||||
wait for TEST_CLOCK_PERIOD;
|
||||
goals_canceling_ren_s <= '0';
|
||||
goals_canceling_ren_c <= '0';
|
||||
wait_on_sig(goals_canceling_valid_s);
|
||||
wait_on_sig(goals_canceling_valid_c);
|
||||
AffirmIfEqual(RR_GOAL_ID, goals_canceling_goal_id_c, goals_canceling_goal_id_r_s);
|
||||
AffirmIfEqual(RR_STAMP, goals_canceling_stamp_c, goals_canceling_stamp_r_s);
|
||||
goals_canceling_ack_s <= '1';
|
||||
goals_canceling_ack_c <= '1';
|
||||
wait for TEST_CLOCK_PERIOD;
|
||||
goals_canceling_ack_s <= '0';
|
||||
goals_canceling_ack_c <= '0';
|
||||
end loop;
|
||||
|
||||
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;
|
||||
494
src/ros2/Tests/Level_1/L1_CancelGoal_ros_srv_test2.vhd
Normal file
494
src/ros2/Tests/Level_1/L1_CancelGoal_ros_srv_test2.vhd
Normal file
@ -0,0 +1,494 @@
|
||||
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;
|
||||
use work.CancelGoal_package.all;
|
||||
|
||||
-- This testbench tests the General Behavour of the AddTwoInts Service.
|
||||
-- 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_CancelGoal_ros_srv_test2 is
|
||||
end entity;
|
||||
|
||||
architecture testbench of L1_CancelGoal_ros_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_info_goal_id_s, goal_info_goal_id_c : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0');
|
||||
signal goal_info_stamp_s, goal_info_stamp_c : std_logic_vector(ROS_TIME_WIDTH-1 downto 0) := (others => '0');
|
||||
signal return_code_s1, return_code_c1 : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0');
|
||||
signal goals_canceling_len_s, goals_canceling_len_c : std_logic_vector(RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0) := (others => '0');
|
||||
signal goals_canceling_addr_s, goals_canceling_addr_c : std_logic_vector(RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0) := (others => '0');
|
||||
signal goals_canceling_ready_s, goals_canceling_ready_c, goals_canceling_ren_s, goals_canceling_ren_c, goals_canceling_wen_s, goals_canceling_valid_s, goals_canceling_valid_c, goals_canceling_ack_s, goals_canceling_ack_c : std_logic := '0';
|
||||
signal goals_canceling_goal_id_r_s, goals_canceling_goal_id_w_s, goals_canceling_goal_id_c : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0');
|
||||
signal goals_canceling_stamp_r_s, goals_canceling_stamp_w_s, goals_canceling_stamp_c : std_logic_vector(ROS_TIME_WIDTH-1 downto 0) := (others => '0');
|
||||
begin
|
||||
|
||||
uut_c : entity work.CancelGoal_ros_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_info_goal_id => goal_info_goal_id_c,
|
||||
goal_info_stamp => goal_info_stamp_c,
|
||||
return_code => return_code_c1,
|
||||
goals_canceling_len => goals_canceling_len_c,
|
||||
goals_canceling_addr => goals_canceling_addr_c,
|
||||
goals_canceling_ready => goals_canceling_ready_c,
|
||||
goals_canceling_ren => goals_canceling_ren_c,
|
||||
goals_canceling_valid => goals_canceling_valid_c,
|
||||
goals_canceling_ack => goals_canceling_ack_c,
|
||||
goals_canceling_goal_id => goals_canceling_goal_id_c,
|
||||
goals_canceling_stamp => goals_canceling_stamp_c,
|
||||
done_user => done_c,--
|
||||
return_code_user => return_code_c --
|
||||
);
|
||||
|
||||
uut_s : entity work.CancelGoal_ros_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_info_goal_id => goal_info_goal_id_s,
|
||||
goal_info_stamp => goal_info_stamp_s,
|
||||
return_code => return_code_s1,
|
||||
goals_canceling_len => goals_canceling_len_s,
|
||||
goals_canceling_addr => goals_canceling_addr_s,
|
||||
goals_canceling_ready => goals_canceling_ready_s,
|
||||
goals_canceling_ren => goals_canceling_ren_s,
|
||||
goals_canceling_wen => goals_canceling_wen_s,
|
||||
goals_canceling_valid => goals_canceling_valid_s,
|
||||
goals_canceling_ack => goals_canceling_ack_s,
|
||||
goals_canceling_goal_id_r => goals_canceling_goal_id_r_s,
|
||||
goals_canceling_goal_id_w => goals_canceling_goal_id_w_s,
|
||||
goals_canceling_stamp_r => goals_canceling_stamp_r_s,
|
||||
goals_canceling_stamp_w => goals_canceling_stamp_w_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 RQ_GOAL_ID, RQ_STAMP, RR_GOAL_ID, RR_STAMP, RR_RETURN_CODE : 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("CancelGoal Service - Level 1 - (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);
|
||||
RQ_GOAL_ID := GetAlertLogID("RQ_GOAL_ID", ALERTLOG_BASE_ID);
|
||||
RQ_STAMP := GetAlertLogID("RQ_STAMP", ALERTLOG_BASE_ID);
|
||||
RR_RETURN_CODE := GetAlertLogID("RR_RETURN_CODE", ALERTLOG_BASE_ID);
|
||||
RR_GOAL_ID := GetAlertLogID("RR_GOAL_ID", ALERTLOG_BASE_ID);
|
||||
RR_STAMP := GetAlertLogID("RR_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_info_goal_id_c <= RV.RandSlv(goal_info_goal_id_c'length);
|
||||
goal_info_stamp_c <= RV.RandSlv(goal_info_stamp_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(RQ_GOAL_ID, goal_info_goal_id_s, goal_info_goal_id_c);
|
||||
AffirmIfEqual(RQ_STAMP, goal_info_stamp_s, goal_info_stamp_c);
|
||||
|
||||
Log("Setting Response", INFO);
|
||||
return_code_s1 <= RV.RandSlv(return_code_s1'length);
|
||||
for i in 0 to 10 loop
|
||||
goals_canceling_len_s <= int(11,goals_canceling_len_s'length);
|
||||
goals_canceling_addr_s <= int(i,goals_canceling_addr_s'length);
|
||||
goals_canceling_goal_id_w_s <= RV.RandSlv(goals_canceling_goal_id_w_s'length);
|
||||
goals_canceling_stamp_w_s <= RV.RandSlv(goals_canceling_stamp_w_s'length);
|
||||
wait_on_sig(goals_canceling_ready_s);
|
||||
goals_canceling_wen_s <= '1';
|
||||
wait for TEST_CLOCK_PERIOD;
|
||||
goals_canceling_wen_s <= '0';
|
||||
end loop;
|
||||
|
||||
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(RR_RETURN_CODE, return_code_c1, return_code_s1);
|
||||
AffirmIfEqual(goals_canceling_len_c, goals_canceling_len_s);
|
||||
for i in 0 to to_integer(unsigned(goals_canceling_len_s))-1 loop
|
||||
goals_canceling_addr_s <= int(i,goals_canceling_addr_s'length);
|
||||
goals_canceling_addr_c <= int(i,goals_canceling_addr_c'length);
|
||||
wait_on_sig(goals_canceling_ready_s);
|
||||
wait_on_sig(goals_canceling_ready_c);
|
||||
goals_canceling_ren_s <= '1';
|
||||
goals_canceling_ren_c <= '1';
|
||||
wait for TEST_CLOCK_PERIOD;
|
||||
goals_canceling_ren_s <= '0';
|
||||
goals_canceling_ren_c <= '0';
|
||||
wait_on_sig(goals_canceling_valid_s);
|
||||
wait_on_sig(goals_canceling_valid_c);
|
||||
AffirmIfEqual(RR_GOAL_ID, goals_canceling_goal_id_c, goals_canceling_goal_id_r_s);
|
||||
AffirmIfEqual(RR_STAMP, goals_canceling_stamp_c, goals_canceling_stamp_r_s);
|
||||
goals_canceling_ack_s <= '1';
|
||||
goals_canceling_ack_c <= '1';
|
||||
wait for TEST_CLOCK_PERIOD;
|
||||
goals_canceling_ack_s <= '0';
|
||||
goals_canceling_ack_c <= '0';
|
||||
end loop;
|
||||
|
||||
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;
|
||||
@ -105,6 +105,11 @@ analyze ../rcl_interfaces/action_msgs/GoalStatusArray_ros_pub.vhd
|
||||
analyze ../rcl_interfaces/action_msgs/GoalStatusArray_ros_sub.vhd
|
||||
analyze Level_1/L1_GoalStatusArray_ros_test1.vhd
|
||||
analyze Level_1/L1_GoalStatusArray_ros_test2.vhd
|
||||
analyze ../rcl_interfaces/action_msgs/CancelGoal_package.vhd
|
||||
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
|
||||
|
||||
|
||||
simulate L1_AddTwoInts_ros_srv_test1
|
||||
@ -114,3 +119,5 @@ 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
|
||||
40
src/ros2/rcl_interfaces/action_msgs/CancelGoal_package.vhd
Normal file
40
src/ros2/rcl_interfaces/action_msgs/CancelGoal_package.vhd
Normal file
@ -0,0 +1,40 @@
|
||||
library ieee;
|
||||
use ieee.std_logic_1164.all;
|
||||
use ieee.numeric_std.all;
|
||||
|
||||
use work.math_pkg.all;
|
||||
use work.rtps_package.all;
|
||||
use work.ros_package.all;
|
||||
use work.GoalInfo_package;
|
||||
|
||||
package CancelGoal_package is
|
||||
-- REQUEST
|
||||
constant RQ_GOAL_INFO_GOAL_ID_MAX_DEPTH : natural := GoalInfo_package.GOAL_ID_MAX_DEPTH;
|
||||
constant RQ_GOAL_INFO_GOAL_ID_ADDR_WIDTH : natural := GoalInfo_package.GOAL_ID_ADDR_WIDTH;
|
||||
|
||||
constant RQ_MAX_GOAL_INFO_GOAL_ID_SIZE : natural := GoalInfo_package.MAX_GOAL_ID_SIZE;
|
||||
constant RQ_MAX_GOAL_INFO_STAMP_SEC_SIZE : natural := GoalInfo_package.MAX_STAMP_SEC_SIZE;
|
||||
constant RQ_MAX_GOAL_INFO_STAMP_NANOSEC_SIZE : natural := GoalInfo_package.MAX_STAMP_NANOSEC_SIZE;
|
||||
constant RQ_MAX_GOAL_INFO_STAMP_SIZE : natural := GoalInfo_package.MAX_STAMP_SIZE;
|
||||
constant RQ_MAX_GOAL_INFO_SIZE : natural := RQ_MAX_GOAL_INFO_GOAL_ID_SIZE + RQ_MAX_GOAL_INFO_STAMP_SIZE;
|
||||
constant RQ_MAX_CANCELGOAL_SIZE : natural := RQ_MAX_GOAL_INFO_SIZE;
|
||||
|
||||
-- RESPONSE
|
||||
constant RR_ERROR_NONE : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(0,CDR_INT8_WIDTH));
|
||||
constant RR_ERROR_REJECTED : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(1,CDR_INT8_WIDTH));
|
||||
constant RR_ERROR_UNKNOWN_GOAL_ID : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(2,CDR_INT8_WIDTH));
|
||||
constant RR_ERROR_GOAL_TERMINATED : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(3,CDR_INT8_WIDTH));
|
||||
|
||||
constant RR_GOALS_CANCELING_GOAL_ID_MAX_DEPTH : natural := GoalInfo_package.GOAL_ID_MAX_DEPTH;
|
||||
constant RR_GOALS_CANCELING_GOAL_ID_ADDR_WIDTH : natural := GoalInfo_package.GOAL_ID_ADDR_WIDTH;
|
||||
constant RR_GOALS_CANCELING_MAX_DEPTH : natural := 100;
|
||||
constant RR_GOALS_CANCELING_ADDR_WIDTH : natural := log2c(RR_GOALS_CANCELING_MAX_DEPTH);
|
||||
|
||||
constant RR_MAX_RETURN_CODE_SIZE : natural := 1;
|
||||
constant RR_MAX_GOALS_CANCELING_GOAL_ID_SIZE : natural := GoalInfo_package.MAX_GOAL_ID_SIZE;
|
||||
constant RR_MAX_GOALS_CANCELING_STAMP_SEC_SIZE : natural := GoalInfo_package.MAX_STAMP_SEC_SIZE;
|
||||
constant RR_MAX_GOALS_CANCELING_STAMP_NANOSEC_SIZE : natural := GoalInfo_package.MAX_STAMP_NANOSEC_SIZE;
|
||||
constant RR_MAX_GOALS_CANCELING_STAMP_SIZE : natural := GoalInfo_package.MAX_STAMP_SIZE;
|
||||
constant RR_MAX_GOALS_CANCELING_SIZE : natural := RR_GOALS_CANCELING_MAX_DEPTH * GoalInfo_package.MAX_GOALINFO_SIZE;
|
||||
constant RR_MAX_CANCELGOAL_SIZE : natural := RR_MAX_RETURN_CODE_SIZE + RR_MAX_GOALS_CANCELING_SIZE;
|
||||
end package;
|
||||
@ -0,0 +1,934 @@
|
||||
-- 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.CancelGoal_package.all;
|
||||
|
||||
entity CancelGoal_ros_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
|
||||
-- ###GENERATED START###
|
||||
goal_info_goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0);
|
||||
goal_info_stamp : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
|
||||
-- ###GENERATED END###
|
||||
-- RESPONSE
|
||||
-- ###GENERATED START###
|
||||
return_code : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
|
||||
goals_canceling_len : out std_logic_vector(RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0);
|
||||
goals_canceling_addr : in std_logic_vector(RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0);
|
||||
goals_canceling_ready : out std_logic;
|
||||
goals_canceling_ren : in std_logic;
|
||||
goals_canceling_valid : out std_logic;
|
||||
goals_canceling_ack : in std_logic;
|
||||
goals_canceling_goal_id : out std_logic_vector(UUID_WIDTH-1 downto 0);
|
||||
goals_canceling_stamp : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
|
||||
-- ###GENERATED END###
|
||||
done_user : out std_logic;
|
||||
return_code_user : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0)
|
||||
);
|
||||
end entity;
|
||||
|
||||
architecture arch of CancelGoal_ros_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_INFO_GOAL_ID,WRITE_RQ_GOAL_INFO_STAMP);
|
||||
type DECODE_STAGE_TYPE is (GET_RID_WGUID,GET_RID_SN,GET_OPTIONAL_HEADER,GET_RR_RETURN_CODE,GET_RR_GOALS_CANCELING_LENGTH,GET_RR_GOALS_CANCELING_GOAL_ID,GET_RR_GOALS_CANCELING_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;
|
||||
-- ###GENERATED START###
|
||||
signal return_code_latch1, return_code_latch1_next : std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
|
||||
signal uuid, uuid_next : UUID_TYPE;
|
||||
signal uuid_cnt, uuid_cnt_next : natural range 0 to RR_GOALS_CANCELING_GOAL_ID_MAX_DEPTH-1;
|
||||
signal goals_canceling_len_latch, goals_canceling_len_latch_next : unsigned(RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0);
|
||||
signal goals_canceling_cnt, goals_canceling_cnt_next : natural range 0 to RR_GOALS_CANCELING_MAX_DEPTH-1;
|
||||
-- goals_canceling_goal_id_mem SIGNALS
|
||||
signal goals_canceling_goal_id_mem_addr : std_logic_vector(RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0);
|
||||
signal goals_canceling_goal_id_mem_read, goals_canceling_goal_id_mem_ready_in, goals_canceling_goal_id_mem_ready_out, goals_canceling_goal_id_mem_valid_in, goals_canceling_goal_id_mem_valid_out : std_logic;
|
||||
signal goals_canceling_goal_id_mem_data_in, goals_canceling_goal_id_mem_data_out : std_logic_vector(UUID_WIDTH-1 downto 0);
|
||||
-- goals_canceling_stamp_mem SIGNALS
|
||||
signal goals_canceling_stamp_mem_addr : std_logic_vector(RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0);
|
||||
signal goals_canceling_stamp_mem_read, goals_canceling_stamp_mem_ready_in, goals_canceling_stamp_mem_ready_out, goals_canceling_stamp_mem_valid_in, goals_canceling_stamp_mem_valid_out : std_logic;
|
||||
signal goals_canceling_stamp_mem_data_in, goals_canceling_stamp_mem_data_out : std_logic_vector(ROS_TIME_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###
|
||||
goals_canceling_goal_id_mem : entity work.mem_ctrl(arch)
|
||||
generic map (
|
||||
ADDR_WIDTH => RR_GOALS_CANCELING_ADDR_WIDTH,
|
||||
DATA_WIDTH => UUID_WIDTH,
|
||||
MEMORY_DEPTH => RR_GOALS_CANCELING_MAX_DEPTH,
|
||||
MAX_BURST_LENGTH => 1
|
||||
)
|
||||
port map (
|
||||
clk => clk,
|
||||
reset => reset or abort_mem,
|
||||
addr => goals_canceling_goal_id_mem_addr,
|
||||
read => goals_canceling_goal_id_mem_read,
|
||||
ready_in => goals_canceling_goal_id_mem_ready_in,
|
||||
valid_in => goals_canceling_goal_id_mem_valid_in,
|
||||
data_in => goals_canceling_goal_id_mem_data_in,
|
||||
ready_out => goals_canceling_goal_id_mem_ready_out,
|
||||
valid_out => goals_canceling_goal_id_mem_valid_out,
|
||||
data_out => goals_canceling_goal_id_mem_data_out
|
||||
);
|
||||
|
||||
goals_canceling_stamp_mem : entity work.mem_ctrl(arch)
|
||||
generic map (
|
||||
ADDR_WIDTH => RR_GOALS_CANCELING_ADDR_WIDTH,
|
||||
DATA_WIDTH => ROS_TIME_WIDTH,
|
||||
MEMORY_DEPTH => RR_GOALS_CANCELING_MAX_DEPTH,
|
||||
MAX_BURST_LENGTH => 1
|
||||
)
|
||||
port map (
|
||||
clk => clk,
|
||||
reset => reset or abort_mem,
|
||||
addr => goals_canceling_stamp_mem_addr,
|
||||
read => goals_canceling_stamp_mem_read,
|
||||
ready_in => goals_canceling_stamp_mem_ready_in,
|
||||
valid_in => goals_canceling_stamp_mem_valid_in,
|
||||
data_in => goals_canceling_stamp_mem_data_in,
|
||||
ready_out => goals_canceling_stamp_mem_ready_out,
|
||||
valid_out => goals_canceling_stamp_mem_valid_out,
|
||||
data_out => goals_canceling_stamp_mem_data_out
|
||||
);
|
||||
-- ###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###
|
||||
return_code <= return_code_latch1;
|
||||
goals_canceling_len <= std_logic_vector(goals_canceling_len_latch);
|
||||
goals_canceling_valid <= goals_canceling_goal_id_mem_valid_out and goals_canceling_stamp_mem_valid_out;
|
||||
goals_canceling_goal_id <= goals_canceling_goal_id_mem_data_out;
|
||||
goals_canceling_stamp <= goals_canceling_stamp_mem_data_out;
|
||||
-- ###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');
|
||||
-- ###GENERATED START###
|
||||
return_code_latch1_next <= return_code_latch1;
|
||||
uuid_cnt_next <= uuid_cnt;
|
||||
uuid <= uuid_next;
|
||||
goals_canceling_ready <= '0';
|
||||
goals_canceling_len_latch_next <= goals_canceling_len_latch;
|
||||
goals_canceling_cnt_next <= goals_canceling_cnt;
|
||||
goals_canceling_goal_id_mem_addr <= (others => '0');
|
||||
goals_canceling_goal_id_mem_read <= '0';
|
||||
goals_canceling_goal_id_mem_valid_in <= '0';
|
||||
goals_canceling_goal_id_mem_ready_out <= '0';
|
||||
goals_canceling_goal_id_mem_data_in <= (others => '0');
|
||||
goals_canceling_stamp_mem_addr <= (others => '0');
|
||||
goals_canceling_stamp_mem_read <= '0';
|
||||
goals_canceling_stamp_mem_valid_in <= '0';
|
||||
goals_canceling_stamp_mem_ready_out <= '0';
|
||||
goals_canceling_stamp_mem_data_in <= (others => '0');
|
||||
-- ###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###
|
||||
goals_canceling_ready <= goals_canceling_goal_id_mem_ready_in and goals_canceling_stamp_mem_ready_in;
|
||||
goals_canceling_goal_id_mem_addr <= goals_canceling_addr;
|
||||
goals_canceling_goal_id_mem_read <= goals_canceling_ren;
|
||||
goals_canceling_goal_id_mem_valid_in <= goals_canceling_ren;
|
||||
goals_canceling_goal_id_mem_ready_out <= goals_canceling_ack;
|
||||
goals_canceling_stamp_mem_addr <= goals_canceling_addr;
|
||||
goals_canceling_stamp_mem_read <= goals_canceling_ren;
|
||||
goals_canceling_stamp_mem_valid_in <= goals_canceling_ren;
|
||||
goals_canceling_stamp_mem_ready_out <= goals_canceling_ack;
|
||||
-- ###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;
|
||||
-- ###GENERATED START###
|
||||
decode_stage_next <= GET_RR_RETURN_CODE;
|
||||
-- ###GENERATED END###
|
||||
when others =>
|
||||
null;
|
||||
end case;
|
||||
end if;
|
||||
-- ###GENERATED START###
|
||||
when GET_RR_RETURN_CODE =>
|
||||
-- ALIGN GUARD
|
||||
if (not check_align(align_offset, ALIGN_1)) then
|
||||
target_align_next <= ALIGN_1;
|
||||
stage_next <= ALIGN_IN_STREAM;
|
||||
else
|
||||
return_code_latch1_next <= endian_swap(endian_flag, get_sub_vector(data_in_latch, to_integer(align_offset(1 downto 0)), CDR_INT8_WIDTH, TRUE));
|
||||
align_offset_next <= align_offset + 1;
|
||||
|
||||
decode_stage_next <= GET_RR_GOALS_CANCELING_LENGTH;
|
||||
|
||||
-- Need to fetch next Word
|
||||
if(align_offset(1 downto 0) = "11") then
|
||||
stage_next <= FETCH;
|
||||
end if;
|
||||
end if;
|
||||
when GET_RR_GOALS_CANCELING_LENGTH =>
|
||||
-- ALIGN GUARD
|
||||
if (not check_align(align_offset, ALIGN_4)) then
|
||||
target_align_next <= ALIGN_4;
|
||||
stage_next <= ALIGN_IN_STREAM;
|
||||
else
|
||||
tmp_length := unsigned(endian_swap(endian_flag, data_in_latch));
|
||||
align_offset_next <= align_offset + 4;
|
||||
stage_next <= FETCH;
|
||||
|
||||
-- Empty Sequence
|
||||
if (tmp_length = 0) then
|
||||
-- DONE
|
||||
stage_next <= SKIP_PAYLOAD;
|
||||
else
|
||||
decode_stage_next <= GET_RR_GOALS_CANCELING_GOAL_ID;
|
||||
cnt_next <= 0;
|
||||
|
||||
if (tmp_length > RR_GOALS_CANCELING_MAX_DEPTH) then
|
||||
goals_canceling_len_latch_next <= to_unsigned(RR_GOALS_CANCELING_MAX_DEPTH, goals_canceling_len_latch_next'length);
|
||||
else
|
||||
goals_canceling_len_latch_next <= resize(tmp_length, goals_canceling_len_latch_next'length);
|
||||
end if;
|
||||
goals_canceling_cnt_next <= 0;
|
||||
end if;
|
||||
end if;
|
||||
when GET_RR_GOALS_CANCELING_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
|
||||
case (cnt) is
|
||||
when 0 =>
|
||||
uuid_next(uuid_cnt) <= get_sub_vector(data_in_latch, to_integer(align_offset(1 downto 0)), CDR_INT8_WIDTH, 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 = RR_GOALS_CANCELING_GOAL_ID_MAX_DEPTH-1) then
|
||||
cnt_next <= cnt + 1;
|
||||
uuid_cnt_next <= 0; -- Post-reset
|
||||
else
|
||||
uuid_cnt_next <= uuid_cnt + 1;
|
||||
end if;
|
||||
when 1 =>
|
||||
goals_canceling_goal_id_mem_addr <= std_logic_vector(to_unsigned(goals_canceling_cnt,RR_GOALS_CANCELING_ADDR_WIDTH));
|
||||
goals_canceling_goal_id_mem_data_in <= std_logic_vector(to_unsigned(uuid));
|
||||
goals_canceling_goal_id_mem_valid_in <= '1';
|
||||
-- Memory Operation Guard
|
||||
if (goals_canceling_goal_id_mem_ready_in = '1') then
|
||||
decode_stage_next <= GET_RR_GOALS_CANCELING_STAMP;
|
||||
cnt_next <= 0;
|
||||
end if;
|
||||
when others =>
|
||||
null;
|
||||
end case;
|
||||
end if;
|
||||
when GET_RR_GOALS_CANCELING_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 =>
|
||||
goals_canceling_stamp_mem_addr <= std_logic_vector(to_unsigned(goals_canceling_cnt,RR_GOALS_CANCELING_ADDR_WIDTH));
|
||||
goals_canceling_stamp_mem_data_in <= dw_latch;
|
||||
goals_canceling_stamp_mem_valid_in <= '1';
|
||||
|
||||
-- Memory Operation Guard
|
||||
if (goals_canceling_stamp_mem_ready_in = '1') then
|
||||
stage_next <= FETCH;
|
||||
align_offset_next <= align_offset + 8;
|
||||
|
||||
-- All Elements processed
|
||||
if (goals_canceling_cnt = goals_canceling_len_latch-1) then
|
||||
-- DONE
|
||||
stage_next <= SKIP_PAYLOAD;
|
||||
else
|
||||
goals_canceling_cnt_next <= goals_canceling_cnt + 1;
|
||||
decode_stage_next <= GET_RR_GOALS_CANCELING_GOAL_ID;
|
||||
cnt_next <= 0;
|
||||
end if;
|
||||
end if;
|
||||
when others =>
|
||||
null;
|
||||
end case;
|
||||
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 <= (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;
|
||||
-- ###GENERATED START###
|
||||
encode_stage_next <= WRITE_RQ_GOAL_INFO_GOAL_ID;
|
||||
-- ###GENERATED END###
|
||||
when others =>
|
||||
end case;
|
||||
end if;
|
||||
-- ###GENERATED START###
|
||||
when WRITE_RQ_GOAL_INFO_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_info_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 = RQ_GOAL_INFO_GOAL_ID_MAX_DEPTH-1) then
|
||||
encode_stage_next <= WRITE_RQ_GOAL_INFO_STAMP;
|
||||
cnt_next <= 0;
|
||||
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;
|
||||
when WRITE_RQ_GOAL_INFO_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(goal_info_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(goal_info_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;
|
||||
-- ###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;
|
||||
-- ###GENERATED START###
|
||||
return_code_latch1 <= (others => '0');
|
||||
uuid <= UUID_UNKNOWN;
|
||||
uuid_cnt <= 0;
|
||||
goals_canceling_cnt <= 0;
|
||||
goals_canceling_len_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;
|
||||
-- ###GENERATED START###
|
||||
return_code_latch1 <= return_code_latch1_next;
|
||||
uuid <= uuid_next;
|
||||
uuid_cnt <= uuid_cnt_next;
|
||||
goals_canceling_cnt <= goals_canceling_cnt_next;
|
||||
goals_canceling_len_latch <= goals_canceling_len_latch_next;
|
||||
-- ###GENERATED END###
|
||||
end if;
|
||||
end if;
|
||||
end process;
|
||||
|
||||
end architecture;
|
||||
@ -0,0 +1,937 @@
|
||||
-- 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.CancelGoal_package.all;
|
||||
|
||||
entity CancelGoal_ros_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
|
||||
-- ###GENERATED START###
|
||||
goal_info_goal_id : out std_logic_vector(UUID_WIDTH-1 downto 0);
|
||||
goal_info_stamp : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
|
||||
-- ###GENERATED END###
|
||||
-- RESPONSE
|
||||
-- ###GENERATED START###
|
||||
return_code : in std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
|
||||
goals_canceling_len : in std_logic_vector(RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0);
|
||||
goals_canceling_addr : in std_logic_vector(RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0);
|
||||
goals_canceling_ready : out std_logic;
|
||||
goals_canceling_ren : in std_logic;
|
||||
goals_canceling_wen : in std_logic;
|
||||
goals_canceling_valid : out std_logic;
|
||||
goals_canceling_ack : in std_logic;
|
||||
goals_canceling_goal_id_r : out std_logic_vector(UUID_WIDTH-1 downto 0);
|
||||
goals_canceling_goal_id_w : in std_logic_vector(UUID_WIDTH-1 downto 0);
|
||||
goals_canceling_stamp_r : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
|
||||
goals_canceling_stamp_w : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
|
||||
-- ###GENERATED END###
|
||||
done_user : out std_logic;
|
||||
return_code_user : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0)
|
||||
);
|
||||
end entity;
|
||||
|
||||
architecture arch of CancelGoal_ros_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_RETURN_CODE,WRITE_RR_GOALS_CANCELING_LENGTH,WRITE_RR_GOALS_CANCELING_GOAL_ID,WRITE_RR_GOALS_CANCELING_STAMP);
|
||||
type DECODE_STAGE_TYPE is (GET_RID_WGUID,GET_RID_SN,GET_OPTIONAL_HEADER,GET_RQ_GOAL_INFO_GOAL_ID,GET_RQ_GOAL_INFO_STAMP);
|
||||
-- TYPE DECLARATIONS
|
||||
-- ###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;
|
||||
-- ###GENERATED START###
|
||||
signal goal_info_goal_id_latch, goal_info_goal_id_latch_next : std_logic_vector(UUID_WIDTH-1 downto 0);
|
||||
signal goal_info_stamp_latch, goal_info_stamp_latch_next : std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
|
||||
signal uuid_cnt, uuid_cnt_next : natural range 0 to RR_GOALS_CANCELING_GOAL_ID_MAX_DEPTH-1;
|
||||
signal goals_canceling_cnt, goals_canceling_cnt_next : natural range 0 to RR_GOALS_CANCELING_MAX_DEPTH-1;
|
||||
-- goals_canceling_goal_id_mem SIGNALS
|
||||
signal goals_canceling_goal_id_mem_addr : std_logic_vector(RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0);
|
||||
signal goals_canceling_goal_id_mem_read, goals_canceling_goal_id_mem_ready_in, goals_canceling_goal_id_mem_ready_out, goals_canceling_goal_id_mem_valid_in, goals_canceling_goal_id_mem_valid_out : std_logic;
|
||||
signal goals_canceling_goal_id_mem_data_in, goals_canceling_goal_id_mem_data_out : std_logic_vector(UUID_WIDTH-1 downto 0);
|
||||
-- goals_canceling_stamp_mem SIGNALS
|
||||
signal goals_canceling_stamp_mem_addr : std_logic_vector(RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0);
|
||||
signal goals_canceling_stamp_mem_read, goals_canceling_stamp_mem_ready_in, goals_canceling_stamp_mem_ready_out, goals_canceling_stamp_mem_valid_in, goals_canceling_stamp_mem_valid_out : std_logic;
|
||||
signal goals_canceling_stamp_mem_data_in, goals_canceling_stamp_mem_data_out : std_logic_vector(ROS_TIME_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###
|
||||
goals_canceling_goal_id_mem : entity work.mem_ctrl(arch)
|
||||
generic map (
|
||||
ADDR_WIDTH => RR_GOALS_CANCELING_ADDR_WIDTH,
|
||||
DATA_WIDTH => UUID_WIDTH,
|
||||
MEMORY_DEPTH => RR_GOALS_CANCELING_MAX_DEPTH,
|
||||
MAX_BURST_LENGTH => 1
|
||||
)
|
||||
port map (
|
||||
clk => clk,
|
||||
reset => reset or abort_mem,
|
||||
addr => goals_canceling_goal_id_mem_addr,
|
||||
read => goals_canceling_goal_id_mem_read,
|
||||
ready_in => goals_canceling_goal_id_mem_ready_in,
|
||||
valid_in => goals_canceling_goal_id_mem_valid_in,
|
||||
data_in => goals_canceling_goal_id_mem_data_in,
|
||||
ready_out => goals_canceling_goal_id_mem_ready_out,
|
||||
valid_out => goals_canceling_goal_id_mem_valid_out,
|
||||
data_out => goals_canceling_goal_id_mem_data_out
|
||||
);
|
||||
|
||||
goals_canceling_stamp_mem : entity work.mem_ctrl(arch)
|
||||
generic map (
|
||||
ADDR_WIDTH => RR_GOALS_CANCELING_ADDR_WIDTH,
|
||||
DATA_WIDTH => ROS_TIME_WIDTH,
|
||||
MEMORY_DEPTH => RR_GOALS_CANCELING_MAX_DEPTH,
|
||||
MAX_BURST_LENGTH => 1
|
||||
)
|
||||
port map (
|
||||
clk => clk,
|
||||
reset => reset or abort_mem,
|
||||
addr => goals_canceling_stamp_mem_addr,
|
||||
read => goals_canceling_stamp_mem_read,
|
||||
ready_in => goals_canceling_stamp_mem_ready_in,
|
||||
valid_in => goals_canceling_stamp_mem_valid_in,
|
||||
data_in => goals_canceling_stamp_mem_data_in,
|
||||
ready_out => goals_canceling_stamp_mem_ready_out,
|
||||
valid_out => goals_canceling_stamp_mem_valid_out,
|
||||
data_out => goals_canceling_stamp_mem_data_out
|
||||
);
|
||||
-- ###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;
|
||||
-- ###GENERATED START###
|
||||
goals_canceling_valid <= goals_canceling_goal_id_mem_valid_out and goals_canceling_stamp_mem_valid_out;
|
||||
goals_canceling_goal_id_r <= goals_canceling_goal_id_mem_data_out;
|
||||
goals_canceling_goal_id_mem_data_in <= goals_canceling_goal_id_w;
|
||||
goals_canceling_stamp_r <= goals_canceling_stamp_mem_data_out;
|
||||
goals_canceling_stamp_mem_data_in <= goals_canceling_stamp_w;
|
||||
goal_info_goal_id <= goal_info_goal_id_latch;
|
||||
goal_info_stamp <= goal_info_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;
|
||||
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');
|
||||
-- ###GENERATED START###
|
||||
goal_info_goal_id_latch_next <= goal_info_goal_id_latch;
|
||||
goal_info_stamp_latch_next <= goal_info_stamp_latch;
|
||||
uuid_cnt_next <= uuid_cnt;
|
||||
goals_canceling_ready <= '0';
|
||||
goals_canceling_cnt_next <= goals_canceling_cnt;
|
||||
goals_canceling_goal_id_mem_addr <= (others => '0');
|
||||
goals_canceling_goal_id_mem_read <= '0';
|
||||
goals_canceling_goal_id_mem_valid_in <= '0';
|
||||
goals_canceling_goal_id_mem_ready_out <= '0';
|
||||
goals_canceling_stamp_mem_addr <= (others => '0');
|
||||
goals_canceling_stamp_mem_read <= '0';
|
||||
goals_canceling_stamp_mem_valid_in <= '0';
|
||||
goals_canceling_stamp_mem_ready_out <= '0';
|
||||
-- ###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###
|
||||
goals_canceling_ready <= goals_canceling_goal_id_mem_ready_in and goals_canceling_stamp_mem_ready_in;
|
||||
goals_canceling_goal_id_mem_addr <= goals_canceling_addr;
|
||||
goals_canceling_goal_id_mem_read <= goals_canceling_ren;
|
||||
goals_canceling_goal_id_mem_valid_in <= goals_canceling_ren or goals_canceling_wen;
|
||||
goals_canceling_goal_id_mem_ready_out <= goals_canceling_ack;
|
||||
goals_canceling_stamp_mem_addr <= goals_canceling_addr;
|
||||
goals_canceling_stamp_mem_read <= goals_canceling_ren;
|
||||
goals_canceling_stamp_mem_valid_in <= goals_canceling_ren or goals_canceling_wen;
|
||||
goals_canceling_stamp_mem_ready_out <= goals_canceling_ack;
|
||||
-- ###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;
|
||||
-- ###GENERATED START###
|
||||
decode_stage_next <= GET_RQ_GOAL_INFO_GOAL_ID;
|
||||
-- ###GENERATED END###
|
||||
when others =>
|
||||
null;
|
||||
end case;
|
||||
end if;
|
||||
-- ###GENERATED START###
|
||||
when GET_RQ_GOAL_INFO_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_info_goal_id_latch_next <= write_sub_vector(goal_info_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 = RQ_GOAL_INFO_GOAL_ID_MAX_DEPTH-1) then
|
||||
decode_stage_next <= GET_RQ_GOAL_INFO_STAMP;
|
||||
cnt_next <= 0;
|
||||
uuid_cnt_next <= 0; -- Post-reset
|
||||
else
|
||||
uuid_cnt_next <= uuid_cnt + 1;
|
||||
end if;
|
||||
end if;
|
||||
when GET_RQ_GOAL_INFO_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 =>
|
||||
goal_info_stamp_latch_next <= write_sub_vector(goal_info_stamp_latch_next, endian_swap(endian_flag, data_in_latch), 0, TRUE);
|
||||
stage_next <= FETCH;
|
||||
cnt_next <= cnt + 1;
|
||||
-- Double Word 2/2
|
||||
when 1 =>
|
||||
goal_info_stamp_latch_next <= write_sub_vector(goal_info_stamp_latch_next, endian_swap(endian_flag, data_in_latch), 1, TRUE);
|
||||
stage_next <= FETCH;
|
||||
align_offset_next <= align_offset + 8;
|
||||
|
||||
-- DONE
|
||||
stage_next <= SKIP_PAYLOAD;
|
||||
when others =>
|
||||
null;
|
||||
end case;
|
||||
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;
|
||||
-- ###GENERATED START###
|
||||
encode_stage_next <= WRITE_RR_RETURN_CODE;
|
||||
-- ###GENERATED END###
|
||||
when others =>
|
||||
end case;
|
||||
end if;
|
||||
-- ###GENERATED START###
|
||||
when WRITE_RR_RETURN_CODE =>
|
||||
-- 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, endian_swap(LITTLE_ENDIAN, return_code), to_integer(align_offset(1 downto 0)), TRUE);
|
||||
align_offset_next <= align_offset + 1;
|
||||
|
||||
encode_stage_next <= WRITE_RR_GOALS_CANCELING_LENGTH;
|
||||
|
||||
-- Need to fetch next Word
|
||||
if(align_offset(1 downto 0) = "11") then
|
||||
stage_next <= PUSH;
|
||||
end if;
|
||||
end if;
|
||||
when WRITE_RR_GOALS_CANCELING_LENGTH =>
|
||||
-- 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, std_logic_vector(resize(unsigned(goals_canceling_len),WORD_WIDTH)));
|
||||
align_offset_next <= align_offset + 4;
|
||||
stage_next <= PUSH;
|
||||
|
||||
-- Empty Sequence
|
||||
if (unsigned(goals_canceling_len) = 0) then
|
||||
-- DONE
|
||||
stage_next <= PUSH;
|
||||
finalize_payload_next <= '1';
|
||||
else
|
||||
encode_stage_next <= WRITE_RR_GOALS_CANCELING_GOAL_ID;
|
||||
goals_canceling_cnt_next <= 0;
|
||||
cnt_next <= 0;
|
||||
end if;
|
||||
end if;
|
||||
when WRITE_RR_GOALS_CANCELING_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
|
||||
case (cnt) is
|
||||
-- GET
|
||||
when 0 =>
|
||||
goals_canceling_goal_id_mem_addr <= std_logic_vector(to_unsigned(goals_canceling_cnt,RR_GOALS_CANCELING_ADDR_WIDTH));
|
||||
goals_canceling_goal_id_mem_valid_in <= '1';
|
||||
goals_canceling_goal_id_mem_read <= '1';
|
||||
-- Memory Operation Guard
|
||||
if (goals_canceling_goal_id_mem_ready_in = '1') then
|
||||
cnt_next <= cnt + 1;
|
||||
uuid_cnt_next <= 0;
|
||||
end if;
|
||||
-- WRITE
|
||||
when 1 =>
|
||||
-- Memory Operation Guard
|
||||
if (goals_canceling_goal_id_mem_valid_out = '1') then
|
||||
data_out_latch_next <= write_sub_vector(data_out_latch_next, get_sub_vector(goals_canceling_goal_id_mem_data_out,uuid_cnt,CDR_INT8_WIDTH,TRUE), to_integer(align_offset(1 downto 0)), TRUE);
|
||||
align_offset_next <= align_offset + 1;
|
||||
|
||||
if (uuid_cnt = RR_GOALS_CANCELING_GOAL_ID_MAX_DEPTH-1) then
|
||||
goals_canceling_goal_id_mem_ready_out <= '1';
|
||||
encode_stage_next <= WRITE_RR_GOALS_CANCELING_STAMP;
|
||||
cnt_next <= 0;
|
||||
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;
|
||||
when others =>
|
||||
end case;
|
||||
end if;
|
||||
when WRITE_RR_GOALS_CANCELING_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
|
||||
-- GET
|
||||
when 0 =>
|
||||
goals_canceling_stamp_mem_addr <= std_logic_vector(to_unsigned(goals_canceling_cnt,RR_GOALS_CANCELING_ADDR_WIDTH));
|
||||
goals_canceling_stamp_mem_valid_in <= '1';
|
||||
goals_canceling_stamp_mem_read <= '1';
|
||||
-- Memory Operation Guard
|
||||
if (goals_canceling_stamp_mem_ready_in = '1') then
|
||||
cnt_next <= cnt + 1;
|
||||
end if;
|
||||
-- WRITE 1/2
|
||||
when 1 =>
|
||||
-- Memory Operation Guard
|
||||
if (goals_canceling_stamp_mem_valid_out = '1') then
|
||||
data_out_latch_next <= endian_swap(LITTLE_ENDIAN, get_sub_vector(goals_canceling_stamp_mem_data_out, 0, WORD_WIDTH, TRUE));
|
||||
stage_next <= PUSH;
|
||||
cnt_next <= cnt + 1;
|
||||
end if;
|
||||
-- WRITE 2/2
|
||||
when 2 =>
|
||||
goals_canceling_stamp_mem_ready_out <= '1';
|
||||
-- Memory Operation Guard
|
||||
if (goals_canceling_stamp_mem_valid_out = '1') then
|
||||
data_out_latch_next <= endian_swap(LITTLE_ENDIAN, get_sub_vector(goals_canceling_stamp_mem_data_out, 1, WORD_WIDTH, TRUE));
|
||||
align_offset_next <= align_offset + 8;
|
||||
stage_next <= PUSH;
|
||||
|
||||
-- All Elements processed
|
||||
if (goals_canceling_cnt = unsigned(goals_canceling_len)-1) then
|
||||
-- DONE
|
||||
stage_next <= PUSH;
|
||||
finalize_payload_next <= '1';
|
||||
else
|
||||
goals_canceling_cnt_next <= goals_canceling_cnt + 1;
|
||||
encode_stage_next <= WRITE_RR_GOALS_CANCELING_GOAL_ID;
|
||||
cnt_next <= 0;
|
||||
end if;
|
||||
end if;
|
||||
when others =>
|
||||
end case;
|
||||
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');
|
||||
return_code_latch <= ROS_RET_OK;
|
||||
service_info_sig <= EMPTY_SERVICE_INFO;
|
||||
-- ###GENERATED START###
|
||||
uuid_cnt <= 0;
|
||||
goals_canceling_cnt <= 0;
|
||||
return_code_latch <= (others => '0');
|
||||
goal_info_goal_id_latch <= (others => '0');
|
||||
goal_info_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;
|
||||
return_code_latch <= return_code_latch_next;
|
||||
service_info_sig <= service_info_sig_next;
|
||||
-- ###GENERATED START###
|
||||
uuid_cnt <= uuid_cnt_next;
|
||||
goals_canceling_cnt <= goals_canceling_cnt_next;
|
||||
return_code_latch <= return_code_latch_next;
|
||||
goal_info_goal_id_latch <= goal_info_goal_id_latch_next;
|
||||
goal_info_stamp_latch <= goal_info_stamp_latch_next;
|
||||
-- ###GENERATED END###
|
||||
end if;
|
||||
end if;
|
||||
end process;
|
||||
|
||||
end architecture;
|
||||
Loading…
Reference in New Issue
Block a user