From 6bbb5fc10266a1aa50937407a55d85a3c0e57956 Mon Sep 17 00:00:00 2001 From: Greek64 Date: Wed, 2 Mar 2022 11:27:57 +0100 Subject: [PATCH] Add ROS Action Server implementation --- sim/L0_ros_action_server_test1.do | 223 ++ sim/L0_ros_action_server_test2.do | 223 ++ src/REF.txt | 63 +- src/TODO.txt | 10 + src/ros2/ROS_IDL.txt | 7 +- .../Level_0/L0_ros_action_server_test1.vhd | 1313 +++++++ .../Level_0/L0_ros_action_server_test2.vhd | 1311 +++++++ .../Tests/Level_0/ros_action_server_tests.txt | 50 + src/ros2/Tests/ros_testbench.pro | 7 +- src/ros2/ros_action_server.vhd | 3327 ++++++++++++++++ .../ros_action_server.vhd.MULTIPROCESS_BAK | 3413 +++++++++++++++++ src/ros2/ros_package.vhd | 76 +- src/rtps_package.vhd | 22 +- 13 files changed, 10037 insertions(+), 8 deletions(-) create mode 100644 sim/L0_ros_action_server_test1.do create mode 100644 sim/L0_ros_action_server_test2.do create mode 100644 src/ros2/Tests/Level_0/L0_ros_action_server_test1.vhd create mode 100644 src/ros2/Tests/Level_0/L0_ros_action_server_test2.vhd create mode 100644 src/ros2/Tests/Level_0/ros_action_server_tests.txt create mode 100644 src/ros2/ros_action_server.vhd create mode 100644 src/ros2/ros_action_server.vhd.MULTIPROCESS_BAK diff --git a/sim/L0_ros_action_server_test1.do b/sim/L0_ros_action_server_test1.do new file mode 100644 index 0000000..139f119 --- /dev/null +++ b/sim/L0_ros_action_server_test1.do @@ -0,0 +1,223 @@ +onerror {resume} +radix define DDS_RETCODE { + "10#0#" "RETCODE_OK", + "10#1#" "RETCODE_ERROR", + "10#2#" "RETCODE_UNSUPPORTED", + "10#3#" "RETCODE_BAD_PARAMETER", + "10#4#" "RETCODE_PRECONDITION_NOT_MET", + "10#5#" "RETCODE_OUT_OF_RESOURCES", + "10#6#" "RETCODE_NOT_ENABLED", + "10#7#" "RETCODE_IMMUTABLE_POLICY", + "10#8#" "RETCODE_INCONSISTENT_POLICY", + "10#9#" "RETCODE_ALREADY_DELETED", + "10#10#" "RETCODE_TIMEOUT", + "10#11#" "RETCODE_NO_DATA", + "10#12#" "RETCODE_ILLEGAL_OPERATION", + -default unsigned +} +radix define ROS_RETCODE { + "10#0#" "ROS_RET_OK", + "10#1#" "ROS_RET_ERROR", + "10#2#" "ROS_RET_TIMEOUT", + "10#3#" "ROS_RET_UNSUPPORTED", + "10#10#" "ROS_RET_BAD_ALLOC", + "10#11#" "ROS_RET_INVALID_ARGUMENT", + "10#12#" "ROS_RET_INCORRECT_RMW_IMPLEMENTATION", + "10#100#" "ROS_RET_ALREADY_INIT", + "10#101#" "ROS_RET_NOT_INIT", + "10#102#" "ROS_RET_MISMATCHED_RMW", + "10#103#" "ROS_RET_TOPIC_NAME_INVALID", + "10#104#" "ROS_RET_SERVICE_NAME_INVALID", + "10#105#" "ROS_RET_UNKNOWN_SUBSTITUTION", + "10#106#" "ROS_RET_ALREADY_SHUTDOWN", + "10#200#" "ROS_RET_NODE_INVALID", + "10#201#" "ROS_RET_NODE_INVALID_NAME", + "10#202#" "ROS_RET_NODE_INVALID_NAMESPACE", + "10#203#" "ROS_RET_NODE_NAME_NON_EXISTENT", + "10#400#" "ROS_RET_SUBSCRIPTION_INVALID", + "10#401#" "ROS_RET_SUBSCRIPTION_TAKE_FAILED", + "10#500#" "ROS_RET_CLIENT_INVALID", + "10#501#" "ROS_RET_CLIENT_TAKE_FAILED", + "10#600#" "ROS_RET_SERVICE_INVALID", + "10#601#" "ROS_RET_SERVICE_TAKE_FAILED", + "10#800#" "ROS_RET_TIMER_INVALID", + "10#801#" "ROS_RET_TIMER_CANCELED", + "10#900#" "ROS_RET_WAIT_SET_INVALID", + "10#901#" "ROS_RET_WAIT_SET_EMPTY", + "10#902#" "ROS_RET_WAIT_SET_FULL", + "10#1001#" "ROS_RET_INVALID_REMAP_RULE", + "10#1002#" "ROS_RET_WRONG_LEXEME", + "10#1003#" "ROS_RET_INVALID_ROS_ARGS", + "10#1010#" "ROS_RET_INVALID_PARAM_RULE", + "10#1020#" "ROS_RET_INVALID_LOG_LEVEL_RULE", + "10#2001#" "ROS_RET_EVENT_TAKE_FAILED", + "10#2000#" "ROS_RET_ACTION_NAME_INVALID", + "10#2100#" "ROS_RET_ACTION_GOAL_ACCEPTED", + "10#2101#" "ROS_RET_ACTION_GOAL_REJECTED", + "10#2102#" "ROS_RET_ACTION_CLIENT_INVALID", + "10#2103#" "ROS_RET_ACTION_CLIENT_TAKE_FAILED", + "10#2200#" "ROS_RET_ACTION_SERVER_INVALID", + "10#2201#" "ROS_RET_ACTION_SERVER_TAKE_FAILED", + "10#2300#" "ROS_RET_ACTION_GOAL_HANDLE_INVALID", + "10#2301#" "ROS_RET_ACTION_GOAL_EVENT_INVALID", + "10#3000#" "ROS_RET_LIFECYCLE_STATE_REGISTERED", + "10#3001#" "ROS_RET_LIFECYCLE_STATE_NOT_REGISTERED", + -default unsigned +} +radix define GOAL_STATUS { + "10#0#" "STATUS_UNKNOWN", + "10#1#" "STATUS_ACCEPTED", + "10#2#" "STATUS_EXECUTING", + "10#3#" "STATUS_CANCELING", + "10#4#" "STATUS_SUCCEEDED", + "10#5#" "STATUS_CANCELED", + "10#6#" "STATUS_ABORTED", + -default unsigned +} +radix define CANCEL_RETCODE { + "10#0#" "ERROR_NONE", + "10#1#" "ERROR_REJECTED", + "10#2#" "ERROR_UNKNOWN_GOAL_ID", + "10#3#" "ERROR_GOAL_TERMINATED", + -default unsigned +} +quietly WaveActivateNextPane {} 0 +add wave -noupdate -divider SYSTEM +add wave -noupdate /l0_ros_action_server_test1/uut/clk +add wave -noupdate /l0_ros_action_server_test1/uut/reset +add wave -noupdate -radix unsigned /l0_ros_action_server_test1/uut/time +add wave -noupdate -radix unsigned /l0_ros_action_server_test1/uut/check_time +add wave -noupdate -divider GOAL +add wave -noupdate -group GOAL /l0_ros_action_server_test1/uut/data_available_g +add wave -noupdate -group GOAL /l0_ros_action_server_test1/uut/start_g +add wave -noupdate -group GOAL /l0_ros_action_server_test1/uut/ack_g +add wave -noupdate -group GOAL /l0_ros_action_server_test1/uut/opcode_g +add wave -noupdate -group GOAL -radix hexadecimal /l0_ros_action_server_test1/uut/service_info_g +add wave -noupdate -group GOAL -radix hexadecimal /l0_ros_action_server_test1/uut/request_id_g +add wave -noupdate -group GOAL /l0_ros_action_server_test1/uut/taken_g +add wave -noupdate -group GOAL /l0_ros_action_server_test1/uut/done_g +add wave -noupdate -group GOAL -radix ROS_RETCODE /l0_ros_action_server_test1/uut/return_code_g +add wave -noupdate -group GOAL -radix hexadecimal /l0_ros_action_server_test1/uut/goal_id_g +add wave -noupdate -group GOAL /l0_ros_action_server_test1/uut/accepted_g +add wave -noupdate -group GOAL -radix hexadecimal /l0_ros_action_server_test1/uut/stamp_g +add wave -noupdate -divider RESULT +add wave -noupdate -group RESULT /l0_ros_action_server_test1/uut/data_available_r +add wave -noupdate -group RESULT /l0_ros_action_server_test1/uut/start_r +add wave -noupdate -group RESULT /l0_ros_action_server_test1/uut/ack_r +add wave -noupdate -group RESULT /l0_ros_action_server_test1/uut/opcode_r +add wave -noupdate -group RESULT -radix hexadecimal /l0_ros_action_server_test1/uut/service_info_r +add wave -noupdate -group RESULT -radix hexadecimal /l0_ros_action_server_test1/uut/request_id_r +add wave -noupdate -group RESULT /l0_ros_action_server_test1/uut/taken_r +add wave -noupdate -group RESULT /l0_ros_action_server_test1/uut/done_r +add wave -noupdate -group RESULT -radix ROS_RETCODE /l0_ros_action_server_test1/uut/return_code_r +add wave -noupdate -group RESULT -radix unsigned /l0_ros_action_server_test1/uut/result_index +add wave -noupdate -group RESULT /l0_ros_action_server_test1/uut/result_sel +add wave -noupdate -group RESULT /l0_ros_action_server_test1/uut/result_sel_ack +add wave -noupdate -group RESULT -radix hexadecimal /l0_ros_action_server_test1/uut/goal_id_r +add wave -noupdate -group RESULT -radix GOAL_STATUS /l0_ros_action_server_test1/uut/status_r +add wave -noupdate -divider CANCEL +add wave -noupdate -group CANCEL /l0_ros_action_server_test1/uut/data_available_c +add wave -noupdate -group CANCEL /l0_ros_action_server_test1/uut/start_c +add wave -noupdate -group CANCEL /l0_ros_action_server_test1/uut/ack_c +add wave -noupdate -group CANCEL /l0_ros_action_server_test1/uut/opcode_c +add wave -noupdate -group CANCEL -radix hexadecimal /l0_ros_action_server_test1/uut/service_info_c +add wave -noupdate -group CANCEL -radix hexadecimal /l0_ros_action_server_test1/uut/request_id_c +add wave -noupdate -group CANCEL /l0_ros_action_server_test1/uut/taken_c +add wave -noupdate -group CANCEL /l0_ros_action_server_test1/uut/done_c +add wave -noupdate -group CANCEL -radix CANCEL_RETCODE /l0_ros_action_server_test1/uut/return_code_c +add wave -noupdate -group CANCEL -radix hexadecimal /l0_ros_action_server_test1/uut/goal_info_goal_id_c +add wave -noupdate -group CANCEL -radix hexadecimal /l0_ros_action_server_test1/uut/goal_info_stamp_c +add wave -noupdate -group CANCEL -radix GOAL_STATUS /l0_ros_action_server_test1/uut/cancel_return_code_c +add wave -noupdate -group CANCEL -radix unsigned /l0_ros_action_server_test1/uut/goals_canceling_len_c +add wave -noupdate -group CANCEL -radix unsigned /l0_ros_action_server_test1/uut/goals_canceling_addr_c +add wave -noupdate -group CANCEL /l0_ros_action_server_test1/uut/goals_canceling_ready_c +add wave -noupdate -group CANCEL /l0_ros_action_server_test1/uut/goals_canceling_wen_c +add wave -noupdate -group CANCEL -radix hexadecimal /l0_ros_action_server_test1/uut/goals_canceling_goal_id_w_c +add wave -noupdate -group CANCEL -radix hexadecimal /l0_ros_action_server_test1/uut/goals_canceling_stamp_w_c +add wave -noupdate -divider STATUS +add wave -noupdate -group STATUS /l0_ros_action_server_test1/uut/start_s +add wave -noupdate -group STATUS /l0_ros_action_server_test1/uut/opcode_s +add wave -noupdate -group STATUS /l0_ros_action_server_test1/uut/ack_s +add wave -noupdate -group STATUS /l0_ros_action_server_test1/uut/done_s +add wave -noupdate -group STATUS -radix ROS_RETCODE /l0_ros_action_server_test1/uut/return_code_s +add wave -noupdate -group STATUS /l0_ros_action_server_test1/uut/status_list_len_s +add wave -noupdate -group STATUS /l0_ros_action_server_test1/uut/status_list_addr_s +add wave -noupdate -group STATUS /l0_ros_action_server_test1/uut/status_list_ready_s +add wave -noupdate -group STATUS /l0_ros_action_server_test1/uut/status_list_wen_s +add wave -noupdate -group STATUS -radix hexadecimal /l0_ros_action_server_test1/uut/status_list_goal_info_goal_id_w_s +add wave -noupdate -group STATUS -radix hexadecimal /l0_ros_action_server_test1/uut/status_list_goal_info_stamp_w_s +add wave -noupdate -group STATUS -radix GOAL_STATUS /l0_ros_action_server_test1/uut/status_list_status_w_s +add wave -noupdate -divider USER +add wave -noupdate -expand -group USER /l0_ros_action_server_test1/uut/start +add wave -noupdate -expand -group USER /l0_ros_action_server_test1/uut/opcode +add wave -noupdate -expand -group USER /l0_ros_action_server_test1/uut/ack +add wave -noupdate -expand -group USER /l0_ros_action_server_test1/uut/done +add wave -noupdate -expand -group USER -radix ROS_RETCODE /l0_ros_action_server_test1/uut/return_code +add wave -noupdate -expand -group USER -radix hexadecimal /l0_ros_action_server_test1/uut/goal_handle_in +add wave -noupdate -expand -group USER -radix hexadecimal /l0_ros_action_server_test1/uut/goal_handle_out +add wave -noupdate -expand -group USER -radix GOAL_STATUS /l0_ros_action_server_test1/uut/goal_state_in +add wave -noupdate -expand -group USER -radix GOAL_STATUS /l0_ros_action_server_test1/uut/goal_state_out +add wave -noupdate -expand -group USER -radix hexadecimal /l0_ros_action_server_test1/uut/goal_id +add wave -noupdate -expand -group USER -radix unsigned /l0_ros_action_server_test1/uut/goal_result_index +add wave -noupdate -expand -group USER /l0_ros_action_server_test1/uut/goal_stamp +add wave -noupdate -expand -group USER /l0_ros_action_server_test1/uut/new_goal_request +add wave -noupdate -expand -group USER -radix hexadecimal /l0_ros_action_server_test1/uut/new_goal_handle +add wave -noupdate -expand -group USER -radix unsigned /l0_ros_action_server_test1/uut/new_goal_result_index +add wave -noupdate -expand -group USER /l0_ros_action_server_test1/uut/new_goal_accepted +add wave -noupdate -expand -group USER /l0_ros_action_server_test1/uut/new_goal_response +add wave -noupdate -expand -group USER /l0_ros_action_server_test1/uut/cancel_request +add wave -noupdate -expand -group USER -radix hexadecimal /l0_ros_action_server_test1/uut/cancel_request_handle +add wave -noupdate -expand -group USER /l0_ros_action_server_test1/uut/cancel_accepted +add wave -noupdate -expand -group USER /l0_ros_action_server_test1/uut/cancel_response +add wave -noupdate -divider FSM +add wave -noupdate /l0_ros_action_server_test1/uut/stage +add wave -noupdate /l0_ros_action_server_test1/uut/cnt +add wave -noupdate /l0_ros_action_server_test1/uut/terminal_cnt +add wave -noupdate -divider MEM +add wave -noupdate /l0_ros_action_server_test1/uut/mem_start +add wave -noupdate /l0_ros_action_server_test1/uut/mem_opcode +add wave -noupdate /l0_ros_action_server_test1/uut/mem_done +add wave -noupdate -childformat {{/l0_ros_action_server_test1/uut/mem_r.addr -radix unsigned} {/l0_ros_action_server_test1/uut/mem_r.goal_id -radix hexadecimal} {/l0_ros_action_server_test1/uut/mem_r.stamp -radix hexadecimal} {/l0_ros_action_server_test1/uut/mem_r.deadline -radix hexadecimal} {/l0_ros_action_server_test1/uut/mem_r.res_ind -radix unsigned} {/l0_ros_action_server_test1/uut/mem_r.state -radix GOAL_STATUS}} -subitemconfig {/l0_ros_action_server_test1/uut/mem_r.addr {-height 15 -radix unsigned} /l0_ros_action_server_test1/uut/mem_r.goal_id {-height 15 -radix hexadecimal} /l0_ros_action_server_test1/uut/mem_r.stamp {-height 15 -radix hexadecimal} /l0_ros_action_server_test1/uut/mem_r.deadline {-height 15 -radix hexadecimal} /l0_ros_action_server_test1/uut/mem_r.res_ind {-height 15 -radix unsigned} /l0_ros_action_server_test1/uut/mem_r.state {-height 15 -radix GOAL_STATUS}} /l0_ros_action_server_test1/uut/mem_r +add wave -noupdate -radix unsigned /l0_ros_action_server_test1/uut/mem_addr_base +add wave -noupdate /l0_ros_action_server_test1/uut/mem_stage +add wave -noupdate /l0_ros_action_server_test1/uut/mem_cnt +add wave -noupdate -group GOAL_MEM -radix unsigned /l0_ros_action_server_test1/uut/goal_mem_ctrl_inst/addr +add wave -noupdate -group GOAL_MEM /l0_ros_action_server_test1/uut/goal_mem_ctrl_inst/read +add wave -noupdate -group GOAL_MEM /l0_ros_action_server_test1/uut/goal_mem_ctrl_inst/ready_in +add wave -noupdate -group GOAL_MEM /l0_ros_action_server_test1/uut/goal_mem_ctrl_inst/valid_in +add wave -noupdate -group GOAL_MEM -radix hexadecimal /l0_ros_action_server_test1/uut/goal_mem_ctrl_inst/data_in +add wave -noupdate -group GOAL_MEM /l0_ros_action_server_test1/uut/goal_mem_ctrl_inst/ready_out +add wave -noupdate -group GOAL_MEM /l0_ros_action_server_test1/uut/goal_mem_ctrl_inst/valid_out +add wave -noupdate -group GOAL_MEM -radix hexadecimal /l0_ros_action_server_test1/uut/goal_mem_ctrl_inst/data_out +add wave -noupdate -group RRQ_MEM -radix unsigned /l0_ros_action_server_test1/uut/rrq_mem_ctrl_inst/addr +add wave -noupdate -group RRQ_MEM /l0_ros_action_server_test1/uut/rrq_mem_ctrl_inst/read +add wave -noupdate -group RRQ_MEM /l0_ros_action_server_test1/uut/rrq_mem_ctrl_inst/ready_in +add wave -noupdate -group RRQ_MEM /l0_ros_action_server_test1/uut/rrq_mem_ctrl_inst/valid_in +add wave -noupdate -group RRQ_MEM -radix hexadecimal /l0_ros_action_server_test1/uut/rrq_mem_ctrl_inst/data_in +add wave -noupdate -group RRQ_MEM /l0_ros_action_server_test1/uut/rrq_mem_ctrl_inst/ready_out +add wave -noupdate -group RRQ_MEM /l0_ros_action_server_test1/uut/rrq_mem_ctrl_inst/valid_out +add wave -noupdate -group RRQ_MEM -radix hexadecimal /l0_ros_action_server_test1/uut/rrq_mem_ctrl_inst/data_out +add wave -noupdate -childformat {{/l0_ros_action_server_test1/uut/mem_data.addr -radix unsigned} {/l0_ros_action_server_test1/uut/mem_data.goal_id -radix hexadecimal} {/l0_ros_action_server_test1/uut/mem_data.stamp -radix hexadecimal} {/l0_ros_action_server_test1/uut/mem_data.deadline -radix hexadecimal} {/l0_ros_action_server_test1/uut/mem_data.res_ind -radix unsigned} {/l0_ros_action_server_test1/uut/mem_data.state -radix GOAL_STATUS}} -subitemconfig {/l0_ros_action_server_test1/uut/mem_data.addr {-height 15 -radix unsigned} /l0_ros_action_server_test1/uut/mem_data.goal_id {-height 15 -radix hexadecimal} /l0_ros_action_server_test1/uut/mem_data.stamp {-height 15 -radix hexadecimal} /l0_ros_action_server_test1/uut/mem_data.deadline {-height 15 -radix hexadecimal} /l0_ros_action_server_test1/uut/mem_data.res_ind {-height 15 -radix unsigned} /l0_ros_action_server_test1/uut/mem_data.state {-height 15 -radix GOAL_STATUS}} /l0_ros_action_server_test1/uut/mem_data +add wave -noupdate -radix unsigned /l0_ros_action_server_test1/uut/mem_occupied_head +add wave -noupdate -radix unsigned /l0_ros_action_server_test1/uut/mem_occupied_tail +add wave -noupdate -radix unsigned /l0_ros_action_server_test1/uut/mem_empty_head +add wave -noupdate -radix unsigned /l0_ros_action_server_test1/uut/mem_empty_tail +add wave -noupdate -divider MISC +TreeUpdate [SetDefaultTree] +WaveRestoreCursors {{Cursor 1} {139775000 ps} 0} +quietly wave cursor active 1 +configure wave -namecolwidth 218 +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 {139284504 ps} {140265496 ps} diff --git a/sim/L0_ros_action_server_test2.do b/sim/L0_ros_action_server_test2.do new file mode 100644 index 0000000..eab0b07 --- /dev/null +++ b/sim/L0_ros_action_server_test2.do @@ -0,0 +1,223 @@ +onerror {resume} +radix define DDS_RETCODE { + "10#0#" "RETCODE_OK", + "10#1#" "RETCODE_ERROR", + "10#2#" "RETCODE_UNSUPPORTED", + "10#3#" "RETCODE_BAD_PARAMETER", + "10#4#" "RETCODE_PRECONDITION_NOT_MET", + "10#5#" "RETCODE_OUT_OF_RESOURCES", + "10#6#" "RETCODE_NOT_ENABLED", + "10#7#" "RETCODE_IMMUTABLE_POLICY", + "10#8#" "RETCODE_INCONSISTENT_POLICY", + "10#9#" "RETCODE_ALREADY_DELETED", + "10#10#" "RETCODE_TIMEOUT", + "10#11#" "RETCODE_NO_DATA", + "10#12#" "RETCODE_ILLEGAL_OPERATION", + -default unsigned +} +radix define ROS_RETCODE { + "10#0#" "ROS_RET_OK", + "10#1#" "ROS_RET_ERROR", + "10#2#" "ROS_RET_TIMEOUT", + "10#3#" "ROS_RET_UNSUPPORTED", + "10#10#" "ROS_RET_BAD_ALLOC", + "10#11#" "ROS_RET_INVALID_ARGUMENT", + "10#12#" "ROS_RET_INCORRECT_RMW_IMPLEMENTATION", + "10#100#" "ROS_RET_ALREADY_INIT", + "10#101#" "ROS_RET_NOT_INIT", + "10#102#" "ROS_RET_MISMATCHED_RMW", + "10#103#" "ROS_RET_TOPIC_NAME_INVALID", + "10#104#" "ROS_RET_SERVICE_NAME_INVALID", + "10#105#" "ROS_RET_UNKNOWN_SUBSTITUTION", + "10#106#" "ROS_RET_ALREADY_SHUTDOWN", + "10#200#" "ROS_RET_NODE_INVALID", + "10#201#" "ROS_RET_NODE_INVALID_NAME", + "10#202#" "ROS_RET_NODE_INVALID_NAMESPACE", + "10#203#" "ROS_RET_NODE_NAME_NON_EXISTENT", + "10#400#" "ROS_RET_SUBSCRIPTION_INVALID", + "10#401#" "ROS_RET_SUBSCRIPTION_TAKE_FAILED", + "10#500#" "ROS_RET_CLIENT_INVALID", + "10#501#" "ROS_RET_CLIENT_TAKE_FAILED", + "10#600#" "ROS_RET_SERVICE_INVALID", + "10#601#" "ROS_RET_SERVICE_TAKE_FAILED", + "10#800#" "ROS_RET_TIMER_INVALID", + "10#801#" "ROS_RET_TIMER_CANCELED", + "10#900#" "ROS_RET_WAIT_SET_INVALID", + "10#901#" "ROS_RET_WAIT_SET_EMPTY", + "10#902#" "ROS_RET_WAIT_SET_FULL", + "10#1001#" "ROS_RET_INVALID_REMAP_RULE", + "10#1002#" "ROS_RET_WRONG_LEXEME", + "10#1003#" "ROS_RET_INVALID_ROS_ARGS", + "10#1010#" "ROS_RET_INVALID_PARAM_RULE", + "10#1020#" "ROS_RET_INVALID_LOG_LEVEL_RULE", + "10#2001#" "ROS_RET_EVENT_TAKE_FAILED", + "10#2000#" "ROS_RET_ACTION_NAME_INVALID", + "10#2100#" "ROS_RET_ACTION_GOAL_ACCEPTED", + "10#2101#" "ROS_RET_ACTION_GOAL_REJECTED", + "10#2102#" "ROS_RET_ACTION_CLIENT_INVALID", + "10#2103#" "ROS_RET_ACTION_CLIENT_TAKE_FAILED", + "10#2200#" "ROS_RET_ACTION_SERVER_INVALID", + "10#2201#" "ROS_RET_ACTION_SERVER_TAKE_FAILED", + "10#2300#" "ROS_RET_ACTION_GOAL_HANDLE_INVALID", + "10#2301#" "ROS_RET_ACTION_GOAL_EVENT_INVALID", + "10#3000#" "ROS_RET_LIFECYCLE_STATE_REGISTERED", + "10#3001#" "ROS_RET_LIFECYCLE_STATE_NOT_REGISTERED", + -default unsigned +} +radix define GOAL_STATUS { + "10#0#" "STATUS_UNKNOWN", + "10#1#" "STATUS_ACCEPTED", + "10#2#" "STATUS_EXECUTING", + "10#3#" "STATUS_CANCELING", + "10#4#" "STATUS_SUCCEEDED", + "10#5#" "STATUS_CANCELED", + "10#6#" "STATUS_ABORTED", + -default unsigned +} +radix define CANCEL_RETCODE { + "10#0#" "ERROR_NONE", + "10#1#" "ERROR_REJECTED", + "10#2#" "ERROR_UNKNOWN_GOAL_ID", + "10#3#" "ERROR_GOAL_TERMINATED", + -default unsigned +} +quietly WaveActivateNextPane {} 0 +add wave -noupdate -divider SYSTEM +add wave -noupdate /l0_ros_action_server_test2/uut/clk +add wave -noupdate /l0_ros_action_server_test2/uut/reset +add wave -noupdate -radix unsigned /l0_ros_action_server_test2/uut/time +add wave -noupdate -radix unsigned /l0_ros_action_server_test2/uut/check_time +add wave -noupdate -divider GOAL +add wave -noupdate -group GOAL /l0_ros_action_server_test2/uut/data_available_g +add wave -noupdate -group GOAL /l0_ros_action_server_test2/uut/start_g +add wave -noupdate -group GOAL /l0_ros_action_server_test2/uut/ack_g +add wave -noupdate -group GOAL /l0_ros_action_server_test2/uut/opcode_g +add wave -noupdate -group GOAL -radix hexadecimal /l0_ros_action_server_test2/uut/service_info_g +add wave -noupdate -group GOAL -radix hexadecimal /l0_ros_action_server_test2/uut/request_id_g +add wave -noupdate -group GOAL /l0_ros_action_server_test2/uut/taken_g +add wave -noupdate -group GOAL /l0_ros_action_server_test2/uut/done_g +add wave -noupdate -group GOAL -radix ROS_RETCODE /l0_ros_action_server_test2/uut/return_code_g +add wave -noupdate -group GOAL -radix hexadecimal /l0_ros_action_server_test2/uut/goal_id_g +add wave -noupdate -group GOAL /l0_ros_action_server_test2/uut/accepted_g +add wave -noupdate -group GOAL -radix hexadecimal /l0_ros_action_server_test2/uut/stamp_g +add wave -noupdate -divider RESULT +add wave -noupdate -group RESULT /l0_ros_action_server_test2/uut/data_available_r +add wave -noupdate -group RESULT /l0_ros_action_server_test2/uut/start_r +add wave -noupdate -group RESULT /l0_ros_action_server_test2/uut/ack_r +add wave -noupdate -group RESULT /l0_ros_action_server_test2/uut/opcode_r +add wave -noupdate -group RESULT -radix hexadecimal /l0_ros_action_server_test2/uut/service_info_r +add wave -noupdate -group RESULT -radix hexadecimal /l0_ros_action_server_test2/uut/request_id_r +add wave -noupdate -group RESULT /l0_ros_action_server_test2/uut/taken_r +add wave -noupdate -group RESULT /l0_ros_action_server_test2/uut/done_r +add wave -noupdate -group RESULT -radix ROS_RETCODE /l0_ros_action_server_test2/uut/return_code_r +add wave -noupdate -group RESULT -radix unsigned /l0_ros_action_server_test2/uut/result_index +add wave -noupdate -group RESULT /l0_ros_action_server_test2/uut/result_sel +add wave -noupdate -group RESULT /l0_ros_action_server_test2/uut/result_sel_ack +add wave -noupdate -group RESULT -radix hexadecimal /l0_ros_action_server_test2/uut/goal_id_r +add wave -noupdate -group RESULT -radix GOAL_STATUS /l0_ros_action_server_test2/uut/status_r +add wave -noupdate -divider CANCEL +add wave -noupdate -group CANCEL /l0_ros_action_server_test2/uut/data_available_c +add wave -noupdate -group CANCEL /l0_ros_action_server_test2/uut/start_c +add wave -noupdate -group CANCEL /l0_ros_action_server_test2/uut/ack_c +add wave -noupdate -group CANCEL /l0_ros_action_server_test2/uut/opcode_c +add wave -noupdate -group CANCEL -radix hexadecimal /l0_ros_action_server_test2/uut/service_info_c +add wave -noupdate -group CANCEL -radix hexadecimal /l0_ros_action_server_test2/uut/request_id_c +add wave -noupdate -group CANCEL /l0_ros_action_server_test2/uut/taken_c +add wave -noupdate -group CANCEL /l0_ros_action_server_test2/uut/done_c +add wave -noupdate -group CANCEL -radix CANCEL_RETCODE /l0_ros_action_server_test2/uut/return_code_c +add wave -noupdate -group CANCEL -radix hexadecimal /l0_ros_action_server_test2/uut/goal_info_goal_id_c +add wave -noupdate -group CANCEL -radix hexadecimal /l0_ros_action_server_test2/uut/goal_info_stamp_c +add wave -noupdate -group CANCEL -radix GOAL_STATUS /l0_ros_action_server_test2/uut/cancel_return_code_c +add wave -noupdate -group CANCEL -radix unsigned /l0_ros_action_server_test2/uut/goals_canceling_len_c +add wave -noupdate -group CANCEL -radix unsigned /l0_ros_action_server_test2/uut/goals_canceling_addr_c +add wave -noupdate -group CANCEL /l0_ros_action_server_test2/uut/goals_canceling_ready_c +add wave -noupdate -group CANCEL /l0_ros_action_server_test2/uut/goals_canceling_wen_c +add wave -noupdate -group CANCEL -radix hexadecimal /l0_ros_action_server_test2/uut/goals_canceling_goal_id_w_c +add wave -noupdate -group CANCEL -radix hexadecimal /l0_ros_action_server_test2/uut/goals_canceling_stamp_w_c +add wave -noupdate -divider STATUS +add wave -noupdate -group STATUS /l0_ros_action_server_test2/uut/start_s +add wave -noupdate -group STATUS /l0_ros_action_server_test2/uut/opcode_s +add wave -noupdate -group STATUS /l0_ros_action_server_test2/uut/ack_s +add wave -noupdate -group STATUS /l0_ros_action_server_test2/uut/done_s +add wave -noupdate -group STATUS -radix ROS_RETCODE /l0_ros_action_server_test2/uut/return_code_s +add wave -noupdate -group STATUS /l0_ros_action_server_test2/uut/status_list_len_s +add wave -noupdate -group STATUS /l0_ros_action_server_test2/uut/status_list_addr_s +add wave -noupdate -group STATUS /l0_ros_action_server_test2/uut/status_list_ready_s +add wave -noupdate -group STATUS /l0_ros_action_server_test2/uut/status_list_wen_s +add wave -noupdate -group STATUS -radix hexadecimal /l0_ros_action_server_test2/uut/status_list_goal_info_goal_id_w_s +add wave -noupdate -group STATUS -radix hexadecimal /l0_ros_action_server_test2/uut/status_list_goal_info_stamp_w_s +add wave -noupdate -group STATUS -radix GOAL_STATUS /l0_ros_action_server_test2/uut/status_list_status_w_s +add wave -noupdate -divider USER +add wave -noupdate -expand -group USER /l0_ros_action_server_test2/uut/start +add wave -noupdate -expand -group USER /l0_ros_action_server_test2/uut/opcode +add wave -noupdate -expand -group USER /l0_ros_action_server_test2/uut/ack +add wave -noupdate -expand -group USER /l0_ros_action_server_test2/uut/done +add wave -noupdate -expand -group USER -radix ROS_RETCODE /l0_ros_action_server_test2/uut/return_code +add wave -noupdate -expand -group USER -radix hexadecimal /l0_ros_action_server_test2/uut/goal_handle_in +add wave -noupdate -expand -group USER -radix hexadecimal /l0_ros_action_server_test2/uut/goal_handle_out +add wave -noupdate -expand -group USER -radix GOAL_STATUS /l0_ros_action_server_test2/uut/goal_state_in +add wave -noupdate -expand -group USER -radix GOAL_STATUS /l0_ros_action_server_test2/uut/goal_state_out +add wave -noupdate -expand -group USER -radix hexadecimal /l0_ros_action_server_test2/uut/goal_id +add wave -noupdate -expand -group USER -radix unsigned /l0_ros_action_server_test2/uut/goal_result_index +add wave -noupdate -expand -group USER /l0_ros_action_server_test2/uut/goal_stamp +add wave -noupdate -expand -group USER /l0_ros_action_server_test2/uut/new_goal_request +add wave -noupdate -expand -group USER -radix hexadecimal /l0_ros_action_server_test2/uut/new_goal_handle +add wave -noupdate -expand -group USER -radix unsigned /l0_ros_action_server_test2/uut/new_goal_result_index +add wave -noupdate -expand -group USER /l0_ros_action_server_test2/uut/new_goal_accepted +add wave -noupdate -expand -group USER /l0_ros_action_server_test2/uut/new_goal_response +add wave -noupdate -expand -group USER /l0_ros_action_server_test2/uut/cancel_request +add wave -noupdate -expand -group USER -radix hexadecimal /l0_ros_action_server_test2/uut/cancel_request_handle +add wave -noupdate -expand -group USER /l0_ros_action_server_test2/uut/cancel_accepted +add wave -noupdate -expand -group USER /l0_ros_action_server_test2/uut/cancel_response +add wave -noupdate -divider FSM +add wave -noupdate /l0_ros_action_server_test2/uut/stage +add wave -noupdate /l0_ros_action_server_test2/uut/cnt +add wave -noupdate /l0_ros_action_server_test2/uut/terminal_cnt +add wave -noupdate -divider MEM +add wave -noupdate /l0_ros_action_server_test2/uut/mem_start +add wave -noupdate /l0_ros_action_server_test2/uut/mem_opcode +add wave -noupdate /l0_ros_action_server_test2/uut/mem_done +add wave -noupdate -childformat {{/l0_ros_action_server_test2/uut/mem_r.addr -radix unsigned} {/l0_ros_action_server_test2/uut/mem_r.goal_id -radix hexadecimal} {/l0_ros_action_server_test2/uut/mem_r.stamp -radix hexadecimal} {/l0_ros_action_server_test2/uut/mem_r.deadline -radix hexadecimal} {/l0_ros_action_server_test2/uut/mem_r.res_ind -radix unsigned} {/l0_ros_action_server_test2/uut/mem_r.state -radix GOAL_STATUS}} -subitemconfig {/l0_ros_action_server_test2/uut/mem_r.addr {-height 15 -radix unsigned} /l0_ros_action_server_test2/uut/mem_r.goal_id {-height 15 -radix hexadecimal} /l0_ros_action_server_test2/uut/mem_r.stamp {-height 15 -radix hexadecimal} /l0_ros_action_server_test2/uut/mem_r.deadline {-height 15 -radix hexadecimal} /l0_ros_action_server_test2/uut/mem_r.res_ind {-height 15 -radix unsigned} /l0_ros_action_server_test2/uut/mem_r.state {-height 15 -radix GOAL_STATUS}} /l0_ros_action_server_test2/uut/mem_r +add wave -noupdate -radix unsigned /l0_ros_action_server_test2/uut/mem_addr_base +add wave -noupdate /l0_ros_action_server_test2/uut/mem_stage +add wave -noupdate /l0_ros_action_server_test2/uut/mem_cnt +add wave -noupdate -group GOAL_MEM -radix unsigned /l0_ros_action_server_test2/uut/goal_mem_ctrl_inst/addr +add wave -noupdate -group GOAL_MEM /l0_ros_action_server_test2/uut/goal_mem_ctrl_inst/read +add wave -noupdate -group GOAL_MEM /l0_ros_action_server_test2/uut/goal_mem_ctrl_inst/ready_in +add wave -noupdate -group GOAL_MEM /l0_ros_action_server_test2/uut/goal_mem_ctrl_inst/valid_in +add wave -noupdate -group GOAL_MEM -radix hexadecimal /l0_ros_action_server_test2/uut/goal_mem_ctrl_inst/data_in +add wave -noupdate -group GOAL_MEM /l0_ros_action_server_test2/uut/goal_mem_ctrl_inst/ready_out +add wave -noupdate -group GOAL_MEM /l0_ros_action_server_test2/uut/goal_mem_ctrl_inst/valid_out +add wave -noupdate -group GOAL_MEM -radix hexadecimal /l0_ros_action_server_test2/uut/goal_mem_ctrl_inst/data_out +add wave -noupdate -group RRQ_MEM -radix unsigned /l0_ros_action_server_test2/uut/rrq_mem_ctrl_inst/addr +add wave -noupdate -group RRQ_MEM /l0_ros_action_server_test2/uut/rrq_mem_ctrl_inst/read +add wave -noupdate -group RRQ_MEM /l0_ros_action_server_test2/uut/rrq_mem_ctrl_inst/ready_in +add wave -noupdate -group RRQ_MEM /l0_ros_action_server_test2/uut/rrq_mem_ctrl_inst/valid_in +add wave -noupdate -group RRQ_MEM -radix hexadecimal /l0_ros_action_server_test2/uut/rrq_mem_ctrl_inst/data_in +add wave -noupdate -group RRQ_MEM /l0_ros_action_server_test2/uut/rrq_mem_ctrl_inst/ready_out +add wave -noupdate -group RRQ_MEM /l0_ros_action_server_test2/uut/rrq_mem_ctrl_inst/valid_out +add wave -noupdate -group RRQ_MEM -radix hexadecimal /l0_ros_action_server_test2/uut/rrq_mem_ctrl_inst/data_out +add wave -noupdate -childformat {{/l0_ros_action_server_test2/uut/mem_data.addr -radix unsigned} {/l0_ros_action_server_test2/uut/mem_data.goal_id -radix hexadecimal} {/l0_ros_action_server_test2/uut/mem_data.stamp -radix hexadecimal} {/l0_ros_action_server_test2/uut/mem_data.deadline -radix hexadecimal} {/l0_ros_action_server_test2/uut/mem_data.res_ind -radix unsigned} {/l0_ros_action_server_test2/uut/mem_data.state -radix GOAL_STATUS}} -subitemconfig {/l0_ros_action_server_test2/uut/mem_data.addr {-height 15 -radix unsigned} /l0_ros_action_server_test2/uut/mem_data.goal_id {-height 15 -radix hexadecimal} /l0_ros_action_server_test2/uut/mem_data.stamp {-height 15 -radix hexadecimal} /l0_ros_action_server_test2/uut/mem_data.deadline {-height 15 -radix hexadecimal} /l0_ros_action_server_test2/uut/mem_data.res_ind {-height 15 -radix unsigned} /l0_ros_action_server_test2/uut/mem_data.state {-height 15 -radix GOAL_STATUS}} /l0_ros_action_server_test2/uut/mem_data +add wave -noupdate -radix unsigned /l0_ros_action_server_test2/uut/mem_occupied_head +add wave -noupdate -radix unsigned /l0_ros_action_server_test2/uut/mem_occupied_tail +add wave -noupdate -radix unsigned /l0_ros_action_server_test2/uut/mem_empty_head +add wave -noupdate -radix unsigned /l0_ros_action_server_test2/uut/mem_empty_tail +add wave -noupdate -divider MISC +TreeUpdate [SetDefaultTree] +WaveRestoreCursors {{Cursor 1} {139775000 ps} 0} +quietly wave cursor active 1 +configure wave -namecolwidth 218 +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 {139284504 ps} {140265496 ps} diff --git a/src/REF.txt b/src/REF.txt index b827504..71a3182 100644 --- a/src/REF.txt +++ b/src/REF.txt @@ -686,6 +686,67 @@ OUTPUT DATA **| | +---------------------------------------------------------------+ +ACTION SERVER +============= + +GOAL +---- + 31............24..............16..............8...............0 + | | | | | + +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + +-----------------------------------------------+---------------+ +00| UNUSED | GOAL_STATE | + +-----------------------------------------------+---------------+ +01| | + + + +02| | + + GOAL_ID + +03| | + + + +04| | + +---------------------------------------------------------------+ +05| | + + STAMP + +06| | + +---------------------------------------------------------------+ +07| | + + DEADLINE + [only if TIMEOUT_DURATION /= DURATION_INFINITE] +08| | + +---------------------------------------------------------------+ +09| RESULT_INDEX | + +---------------------------------------------------------------+ +10| NEXT_ADDRESS | + +---------------------------------------------------------------+ +11| PREV_ADDRESS | + +---------------------------------------------------------------+ + + + +RESULT_REQUEST +-------------- + 31............24..............16..............8...............0 + | | | | | + +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + +---------------------------------------------------------------+ +00| GOAL_HANDLE | + +---------------------------------------------------------------+ +01| | + + + +02| | + + + +03| | + + REQUEST_ID + +04| | + + + +05| | + + + +06| | + +---------------------------------------------------------------+ +07| NEXT_ADDRESS | + +---------------------------------------------------------------+ +08| PREV_ADDRESS | + +---------------------------------------------------------------+ + TOPIC KEYS ========== @@ -948,4 +1009,4 @@ Parameter List (8.3.5.9) * There shall be no more than 2^16 possible values of the ParameterId_t parameterId * A range of 2^15 values is reserved for protocol-defined parameters * A range of 2^15 values is reserved for vendor-defined parameters -* The maximum length of any parameter is limited to 2^16 octets \ No newline at end of file +* The maximum length of any parameter is limited to 2^16 octets diff --git a/src/TODO.txt b/src/TODO.txt index 6cc6ae7..67db7fc 100644 --- a/src/TODO.txt +++ b/src/TODO.txt @@ -428,6 +428,16 @@ DESIGN DECISIONS does enforce those limits implicitly, since it encodes the memory/registers that are physically limited to those specified upper bounds during static generation. +* RTPS/DDS use a time representation of 64-bit seconds in 32Q32 fixed point format. The ROS libraries + use a 64-bit unsigned nanosecond representation, and ROS sends time (defined in + rcl_interfaces/builtin_interfaces) in 32-bit second and 32-bit nanosecond respresentation. + An internal representation of a 64-bit nanosecond counter seems like the most sensible, but conversions + between are quite resource and latency heavy. + Since the ros action server directly interfaces ros services with the builtin_interfaces definition, + it was decided that the entire server works on this representation to avoid costly converions. This in + effect mitigates the converion problem to the instantiating entity, but a single conversion point could + be defined that can be used throughout the system. + BRAINSTORMING ------------- diff --git a/src/ros2/ROS_IDL.txt b/src/ros2/ROS_IDL.txt index e14d229..a745223 100644 --- a/src/ros2/ROS_IDL.txt +++ b/src/ros2/ROS_IDL.txt @@ -18,4 +18,9 @@ The code generation follows the normal code generation for encoding and decoding * The _package calculates both a maximum request and response message size, named MAX__RQ_SIZE and MAX__RR_SIZE, respectively. The maximum includes the service overhead bytes that are contained in the DDS Payloads. The constant - SERVICE_OVERHEAD_BYTES from ros_package gives this overhead bytes. \ No newline at end of file + SERVICE_OVERHEAD_BYTES from ros_package gives this overhead bytes. + + +ACTION +###### + diff --git a/src/ros2/Tests/Level_0/L0_ros_action_server_test1.vhd b/src/ros2/Tests/Level_0/L0_ros_action_server_test1.vhd new file mode 100644 index 0000000..45e742e --- /dev/null +++ b/src/ros2/Tests/Level_0/L0_ros_action_server_test1.vhd @@ -0,0 +1,1313 @@ +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.GoalStatus_package.all; +use work.CancelGoal_package; +use work.GoalStatusArray_package; + +-- NOTE: Even though this Test instantiates other components, we only (mis-) use them for the memory they instantiate, and thus this Test only tests the uut device (Level 0) + +-- This testbench tests the General Behavour of ROS Action Server. +-- More specifically following tests are done: +-- TEST: NEW GOAL [MEMORY EMPTY] +-- TEST: NEW GOAL [ACCEPT] +-- TEST: NEW GOAL [REJECT] +-- TEST: NEW GOAL [FULL GOAL MEMORY, TIMEOUT/=INFINITE] +-- TEST: GET_GOAL +-- TEST: GET_GOAL [INVALID HANDLE] +-- TEST: GET_LAST_GOAL [MEMORY NOT EMPTY] +-- TEST: GET_LAST_GOAL [MEMORY EMPTY] +-- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL EXISTS] +-- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL DOES NOT EXISTS] +-- TEST: GET_PREVIOUS_GOAL [INVALID HANDLE] +-- TEST: UPDATE_GOAL_STATE [VALID STATE] +-- TEST: UPDATE_GOAL_STATE [INVALID STATE] +-- TEST: UPDATE_GOAL_STATE [INVALID HANDLE] +-- TEST: PUBLISH_FEEDBACK +-- TEST: NEW RESULT REQUEST [MEMORY EMPTY] +-- TEST: NEW RESULT REQUEST [GOAL DOES NOT EXIST] +-- TEST: NEW RESULT REQUEST [GOAL TERMINAL] +-- TEST: NEW RESULT REQUEST [GOAL NOT TERMINAL] +-- TEST: NEW RESULT REQUEST [FULL RESULT MEMORY] +-- TEST: STORED RESULT REQUEST [GOAL STATE UPDATES] +-- TEST: STORED RESULT REQUEST [GOAL STATE CHANGES TO TERMINAL] +-- TEST: STORED RESULT REQUEST [MULTIPLE REQUESTS, SAME GOAL, GOAL STATE CHANGES TO TERMINAL] +-- TEST: EXPIRE GOALS [SINGLE EXPIRED GOAL] +-- TEST: EXPIRE GOALS [MULTIPLE EXPIRED GOALS] +-- TEST: EXPIRE GOALS [ALL GOALS EXPIRED] +-- TEST: GOAL MEMORY [EMPTY->FULL->EMPTY->FULL] +-- TEST: RESULT REQUEST MEMORY [EMPTY->FULL->EMPTY->FULL] +-- TEST: CANCEL REQUEST [MEMORY EMPTY] +-- TEST: CANCEL REQUEST [GOAL ID SET, GOAL EXISTS] +-- TEST: CANCEL REQUEST [GOAL ID SET, GOAL EXISTS, GOAL TERMINAL] +-- TEST: CANCEL REQUEST [GOAL ID SET, GOAL DOES NOT EXISTS] +-- TEST: CANCEL REQUEST [TIME SET, NO RELEVANT GOALS] +-- TEST: CANCEL REQUEST [TIME SET, RELEVANT GOALS EXIST] +-- TEST: CANCEL REQUEST [NO GOAL ID SET, NO TIME SET, GOALS EXIST] +-- TEST: CANCEL REQUEST [NO GOAL ID SET, NO TIME SET, NO GOALS EXIST] +-- TEST: CANCEL REQUEST [GOAL ID SET, TIME SET, GOAL ID IS NOT IN TIME RANGE] +-- TEST: CANCEL REQUEST [GOAL ID SET, TIME SET, GOAL ID IS IN TIME RANGE] +-- TEST: STATUS UPDATE [NEW GOAL] +-- TEST: STATUS UPDATE [GOAL UPDATE] +-- TEST: STATUS UPDATE [GOAL REMOVED] +-- TEST: STATUS UPDATE [GOAL CANCELED] + +entity L0_ros_action_server_test1 is +end entity; + +architecture testbench of L0_ros_action_server_test1 is + + constant MAX_GOALS : natural := 4; + constant MAX_RESULT_REQUESTS : natural := 2; + + type STATUS_CHECK_STAGE_TYPE is (IDLE, CHECK_1, CHECK_2, DONE_STATUS); + + type TEST_GOAL_TYPE is record + goal_id : std_logic_vector(UUID_WIDTH-1 downto 0); + stamp : std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + state : std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + handle : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); + index : std_logic_vector(WORD_WIDTH-1 downto 0); + end record; + type TEST_GOAL_ARRAY_TYPE is array (0 to MAX_GOALS-1) of TEST_GOAL_TYPE; + type TEST_CANCEL_ARRAY_TYPE is array (0 to MAX_GOALS-1) of natural; + + constant ZERO_TEST_GOAL : TEST_GOAL_TYPE := ( + goal_id => (others => '0'), + stamp => (others => '0'), + state => STATUS_UNKNOWN, + handle => GOAL_HANDLE_UNKNOWN, + index => (others => '0') + ); + + type REQUEST_ID_ARRAY_TYPE is array (0 to MAX_RESULT_REQUESTS-1) of REQUEST_ID_TYPE; + + constant ZERO_SAMPLE_INFO : SAMPLE_INFO_TYPE := ( + sample_state => ANY_SAMPLE_STATE, + view_state => ANY_VIEW_STATE, + instance_state => ANY_INSTANCE_STATE, + source_timestamp => TIME_ZERO, + instance_handle => HANDLE_NIL, + publication_handle => HANDLE_NIL, + disposed_generation_count => (others => '0'), + no_writers_generation_count => (others => '0'), + sample_rank => (others => '0'), + generation_rank => (others => '0'), + absolute_generation_rank => (others => '0'), + valid_data => '0' + ); + + signal clk, reset, selector_s, selector_c : std_logic := '0'; + signal t : ROS_TIME_TYPE := ROS_TIME_ZERO; + signal status_stage : STATUS_CHECK_STAGE_TYPE := IDLE; + + signal start_g, ack_g, data_available_g, taken_g, done_g, accepted_g : std_logic := '0'; + signal opcode_g : ROS_SERVICE_OPCODE_TYPE := NOP; + signal service_info_g : SERVICE_INFO_TYPE := EMPTY_SERVICE_INFO; + signal request_id_g : REQUEST_ID_TYPE := EMPTY_REQUEST_ID; + signal return_code_g : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := ROS_RET_OK; + signal goal_id_g : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0'); + signal stamp_g : std_logic_vector(ROS_TIME_WIDTH-1 downto 0) := (others => '0'); + + signal start_r, ack_r, data_available_r, taken_r, done_r, result_sel, result_sel_ack : std_logic := '0'; + signal opcode_r : ROS_SERVICE_OPCODE_TYPE := NOP; + signal service_info_r : SERVICE_INFO_TYPE := EMPTY_SERVICE_INFO; + signal request_id_r : REQUEST_ID_TYPE := EMPTY_REQUEST_ID; + signal return_code_r : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := ROS_RET_OK; + signal result_index : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0'); + signal goal_id_r : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0'); + signal status_r : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0'); + + signal start_c, ack_c, data_available_c, taken_c, done_c : std_logic := '0'; + signal opcode_c : ROS_SERVICE_OPCODE_TYPE := NOP; + signal service_info_c : SERVICE_INFO_TYPE := EMPTY_SERVICE_INFO; + signal request_id_c : REQUEST_ID_TYPE := EMPTY_REQUEST_ID; + signal return_code_c : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := ROS_RET_OK; + signal goal_info_goal_id_c, goals_canceling_goal_id_r, goals_canceling_goal_id_w : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0'); + signal goal_info_stamp_c : std_logic_vector(ROS_TIME_WIDTH-1 downto 0) := (others => '0'); + signal cancel_return_code_c : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0'); + signal goals_canceling_len, goals_canceling_addr, goals_canceling_addr_c, goals_canceling_addr_u : std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0) := (others => '0'); + signal goals_canceling_ready, goals_canceling_ren, goals_canceling_wen, goals_canceling_valid, goals_canceling_ack : std_logic := '0'; + signal goals_canceling_stamp_r, goals_canceling_stamp_w : std_logic_vector(ROS_TIME_WIDTH-1 downto 0) := (others => '0'); + + signal start_fb, ack_fb, done_fb : std_logic := '0'; + signal opcode_fb : ROS_TOPIC_OPCODE_TYPE := NOP; + signal return_code_fb : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0'); + signal goal_id_fb : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0'); + + signal start_s, ack_s, done_s : std_logic := '0'; + signal opcode_s : ROS_TOPIC_OPCODE_TYPE := NOP; + signal return_code_s : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0'); + signal status_list_len, status_list_addr, status_list_addr_s, status_list_addr_u : std_logic_vector(GoalStatusArray_package.STATUS_LIST_ADDR_WIDTH-1 downto 0) := (others => '0'); + signal status_list_ready, status_list_ren, status_list_wen, status_list_valid, status_list_ack : std_logic := '0'; + signal status_list_goal_info_goal_id_r, status_list_goal_info_goal_id_w : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0'); + signal status_list_goal_info_stamp_r, status_list_goal_info_stamp_w : std_logic_vector(ROS_TIME_WIDTH-1 downto 0) := (others => '0'); + signal status_list_status_r, status_list_status_w : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0'); + + signal start, ack, done : std_logic := '0'; + signal opcode : ROS_ACTION_OPCODE_TYPE := NOP; + signal return_code : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0'); + signal goal_handle_in, goal_handle_out : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0) := (others => '0'); + signal goal_state_in, goal_state_out : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0'); + signal goal_id : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0'); + signal goal_result_index : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0'); + signal goal_stamp : ROS_TIME_TYPE := ROS_TIME_ZERO; + signal new_goal_request, new_goal_accepted, new_goal_response, cancel_request, cancel_accepted, cancel_response : std_logic := '0'; + signal new_goal_handle, cancel_request_handle : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0) := (others => '0'); + signal new_goal_result_index : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0'); + + shared variable test_goals : TEST_GOAL_ARRAY_TYPE := (others => ZERO_TEST_GOAL); + shared variable test_goal_cnt : natural := 0; + shared variable GOAL, RESULT, FEEDBACK, STATUS, USER, CANCEL : AlertLogIDType; + + function from_request_id(input : REQUEST_ID_TYPE) return std_logic_vector is + variable ret : std_logic_vector(GUID_WIDTH+63 downto 0); + begin + ret(GUID_WIDTH+63 downto 64) := std_logic_vector(to_unsigned(input.writer_guid)); + ret(63 downto 0) := std_logic_vector(to_unsigned(input.sequence_number)); + return ret; + end function; + +begin + + uut : entity work.ros_action_server(arch) + generic map ( + TIMEOUT_DURATION => gen_duration(20 sec), + MAX_GOALS => MAX_GOALS, + MAX_RESULT_REQUESTS => 2 + ) + port map ( + clk => clk, + reset => reset, + time => t, + start_g => start_g, + ack_g => ack_g, + opcode_g => opcode_g, + service_info_g => service_info_g, + request_id_g => request_id_g, + data_available_g => data_available_g, + taken_g => taken_g, + done_g => done_g, + return_code_g => return_code_g, + goal_id_g => goal_id_g, + accepted_g => accepted_g, + stamp_g => stamp_g, + start_r => start_r, + ack_r => ack_r, + opcode_r => opcode_r, + service_info_r => service_info_r, + request_id_r => request_id_r, + data_available_r => data_available_r, + taken_r => taken_r, + done_r => done_r, + return_code_r => return_code_r, + result_index => result_index, + result_sel => result_sel, + result_sel_ack => result_sel_ack, + goal_id_r => goal_id_r, + status_r => status_r, + start_c => start_c, + ack_c => ack_c, + opcode_c => opcode_c, + service_info_c => service_info_c, + request_id_c => request_id_c, + data_available_c => data_available_c, + taken_c => taken_c, + done_c => done_c, + return_code_c => return_code_c, + goal_info_goal_id_c => goal_info_goal_id_c, + goal_info_stamp_c => goal_info_stamp_c, + cancel_return_code_c => cancel_return_code_c, + goals_canceling_len_c => goals_canceling_len, + goals_canceling_addr_c => goals_canceling_addr_c, + goals_canceling_ready_c => goals_canceling_ready, + goals_canceling_ren_c => open, + goals_canceling_wen_c => goals_canceling_wen, + goals_canceling_valid_c => goals_canceling_valid, + goals_canceling_ack_c => open, + goals_canceling_goal_id_r_c => (others => '0'), + goals_canceling_goal_id_w_c => goals_canceling_goal_id_w, + goals_canceling_stamp_r_c => (others => '0'), + goals_canceling_stamp_w_c => goals_canceling_stamp_w, + start_fb => start_fb, + opcode_fb => opcode_fb, + ack_fb => ack_fb, + done_fb => done_fb, + return_code_fb => return_code_fb, + goal_id_fb => goal_id_fb, + start_s => start_s, + opcode_s => opcode_s, + ack_s => ack_s, + done_s => done_s, + return_code_s => return_code_s, + status_list_len_s => status_list_len, + status_list_addr_s => status_list_addr_s, + status_list_ready_s => status_list_ready, + status_list_ren_s => open, + status_list_wen_s => status_list_wen, + status_list_valid_s => status_list_valid, + status_list_ack_s => open, + status_list_goal_info_goal_id_r_s => (others => '0'), + status_list_goal_info_goal_id_w_s => status_list_goal_info_goal_id_w, + status_list_goal_info_stamp_r_s => (others => '0'), + status_list_goal_info_stamp_w_s => status_list_goal_info_stamp_w, + status_list_status_r_s => status_list_status_r, + status_list_status_w_s => status_list_status_w, + start => start, + opcode => opcode, + ack => ack, + done => done, + return_code => return_code, + goal_handle_in => goal_handle_in, + goal_handle_out => goal_handle_out, + goal_state_in => goal_state_in, + goal_state_out => goal_state_out, + goal_id => goal_id, + goal_result_index => goal_result_index, + goal_stamp => goal_stamp, + new_goal_request => new_goal_request, + new_goal_handle => new_goal_handle, + new_goal_result_index => new_goal_result_index, + new_goal_accepted => new_goal_accepted, + new_goal_response => new_goal_response, + cancel_request => cancel_request, + cancel_request_handle => cancel_request_handle, + cancel_accepted => cancel_accepted, + cancel_response => cancel_response + ); + + cancel_array : entity work.CancelGoal_ros_srv_server(arch) + port map ( + clk => clk, + reset => reset, + start_r => open, + ack_r => '0', + 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 => '0', + return_code_r => (others => '0'), + valid_in_r => '0', + ready_in_r => open, + data_in_r => (others => '0'), + last_word_in_r => '0', + sample_info_r => ZERO_SAMPLE_INFO, + sample_info_valid_r => '0', + sample_info_ack_r => open, + eoc_r => '0', + status_r => (others => '0'), + start_w => open, + ack_w => '0', + opcode_w => open, + instance_handle_out_w => open, + source_ts_w => open, + max_wait_w => open, + done_w => '0', + return_code_w => (others => '0'), + instance_handle_in_w => HANDLE_NIL, + valid_out_w => open, + ready_out_w => '0', + data_out_w => open, + last_word_out_w => open, + valid_in_w => '0', + ready_in_w => open, + data_in_w => (others => '0'), + last_word_in_w => '0', + status_w => (others => '0'), + start_user => '0', + ack_user => open, + opcode_user => NOP, + service_info_user => open, + request_id_user => EMPTY_REQUEST_ID, + data_available_user => open, + taken_user => open, + goal_info_goal_id => open, + goal_info_stamp => open, + return_code => (others => '0'), + goals_canceling_len => goals_canceling_len, + goals_canceling_addr => goals_canceling_addr, + goals_canceling_ready => goals_canceling_ready, + goals_canceling_ren => goals_canceling_ren, + goals_canceling_wen => goals_canceling_wen, + goals_canceling_valid => goals_canceling_valid, + goals_canceling_ack => goals_canceling_ack, + goals_canceling_goal_id_r => goals_canceling_goal_id_r, + goals_canceling_goal_id_w => goals_canceling_goal_id_w, + goals_canceling_stamp_r => goals_canceling_stamp_r, + goals_canceling_stamp_w => goals_canceling_stamp_w, + done_user => open, + return_code_user => open + ); + + status_array : entity work.GoalStatusArray_ros_pub(arch) + port map ( + clk => clk, + reset => reset, + start_dds => open, + ack_dds => '0', + opcode_dds => open, + instance_handle_out_dds => open, + source_ts_dds => open, + max_wait_dds => open, + done_dds => '0', + return_code_dds => (others => '0'), + instance_handle_in_dds => HANDLE_NIL, + valid_out_dds => open, + ready_out_dds => '0', + data_out_dds => open, + last_word_out_dds => open, + valid_in_dds => '0', + ready_in_dds => open, + data_in_dds => (others => '0'), + last_word_in_dds => '0', + status_dds => (others => '0'), + start_user => '0', + opcode_user => NOP, + ack_user => open, + status_list_len => status_list_len, + status_list_addr => status_list_addr, + status_list_ready => status_list_ready, + status_list_ren => status_list_ren, + status_list_wen => status_list_wen, + status_list_valid => status_list_valid, + status_list_ack => status_list_ack, + status_list_goal_info_goal_id_r => status_list_goal_info_goal_id_r, + status_list_goal_info_goal_id_w => status_list_goal_info_goal_id_w, + status_list_goal_info_stamp_r => status_list_goal_info_stamp_r, + status_list_goal_info_stamp_w => status_list_goal_info_stamp_w, + status_list_status_r => status_list_status_r, + status_list_status_w => status_list_status_w, + done_user => open, + return_code_user => open + ); + + status_list_addr <= status_list_addr_s when (selector_s = '0') else status_list_addr_u; + goals_canceling_addr <= goals_canceling_addr_c when (selector_c = '0') else goals_canceling_addr_u; + + stimulus_prc : process + variable RV : RandomPType; + variable test_rrq : REQUEST_ID_ARRAY_TYPE := (others => EMPTY_REQUEST_ID); + variable test_cancel : TEST_CANCEL_ARRAY_TYPE := (others => 0); + + alias idle_sig is <>; + alias mem_done is <>; + + procedure wait_on_sig(signal sig : std_logic) is + begin + if (sig /= '1') then + wait on sig until sig = '1'; + end if; + end procedure; + + impure function gen_request_id return REQUEST_ID_TYPE is + variable ret : REQUEST_ID_TYPE := EMPTY_REQUEST_ID; + begin + ret.writer_guid := to_guid(RV.RandSlv(GUID_WIDTH)); + ret.sequence_number(0) := unsigned(RV.RandSlv(ret.sequence_number(0)'length)); + ret.sequence_number(1) := unsigned(RV.RandSlv(ret.sequence_number(1)'length)); + return ret; + end function; + + impure function gen_goal_id return std_logic_vector is + begin + return RV.RandSlv(UUID_WIDTH); + end function; + + impure function cancel_len return natural is + variable ret : natural := 0; + begin + for i in 0 to MAX_GOALS-1 loop + if (test_cancel(i) = MAX_GOALS) then + exit; + end if; + ret := ret + 1; + end loop; + return ret; + end function; + + function ros_gen_time(t : time) return std_logic_vector is + variable tmp : ROS_TIME_TYPE; + begin + tmp := gen_duration(t); + return std_logic_vector(to_unsigned(tmp)); + end function; + + procedure get_goal_request(index : in natural; goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0)) is + begin + data_available_g <= '1'; + wait_on_sig(start_g); + wait for 1 ps; -- Make sure all signals stable + AlertIf(opcode_g /= TAKE_REQUEST, "Goal service opcode incorrect", FAILURE); + ack_g <= '1'; + wait until rising_edge(clk); + ack_g <= '0'; + service_info_g.request_id <= gen_request_id; + goal_id_g <= goal_id; + data_available_g <= '0'; + done_g <= '1'; + taken_g <= '1'; + return_code_g <= RETCODE_OK; + wait until rising_edge(clk); + done_g <= '0'; + wait until rising_edge(clk); + if (index < test_goals'length) then + test_goals(index).goal_id := goal_id_g; + end if; + end procedure; + + procedure user_input_goal(index : in natural; accept : in boolean) is + begin + wait_on_sig(new_goal_request); + AlertIf((to_integer(unsigned(new_goal_handle)) / 12) /= to_integer(unsigned(new_goal_result_index)), "Result index out of sync", FAILURE); + if (accept) then + test_goals(index).handle := new_goal_handle; + test_goals(index).index := new_goal_result_index; + end if; + new_goal_accepted <= '1' when accept else '0'; + new_goal_response <= '1'; + wait until rising_edge(clk); + if (accept) then + test_goals(index).stamp := std_logic_vector(to_unsigned(t)); + test_goals(index).state := STATUS_ACCEPTED; + test_goal_cnt := test_goal_cnt + 1; + end if; + new_goal_accepted <= '0'; + new_goal_response <= '0'; + wait until rising_edge(clk); + end procedure; + + procedure get_goal_response(index : in natural) is + begin + wait_on_sig(start_g); + AlertIf(opcode_g /= SEND_RESPONSE, "Goal service opcode incorrect", FAILURE); + ack_g <= '1'; + wait until rising_edge(clk); + ack_g <= '0'; + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(GOAL, from_request_id(request_id_g), from_request_id(service_info_g.request_id)); + if (index < test_goals'length) then + AffirmIfEqual(GOAL, accepted_g, '1'); + else + AffirmIfEqual(GOAL, accepted_g, '0'); + end if; + --AffirmIfEqual(GOAL, stamp_g, test_goals(index).stamp); + AffirmIfEqual(GOAL, stamp_g, std_logic_vector(to_unsigned(t))); + done_g <= '1'; + return_code_g <= RETCODE_OK; + wait until rising_edge(clk); + done_g <= '0'; + wait until rising_edge(clk); + end procedure; + + procedure get_result_request(index : in natural; goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0)) is + begin + data_available_r <= '1'; + wait_on_sig(start_r); + wait for 1 ps; -- Make sure all signals stable + AlertIf(opcode_r /= TAKE_REQUEST, "Result service opcode incorrect", FAILURE); + ack_r <= '1'; + wait until rising_edge(clk); + ack_r <= '0'; + service_info_r.request_id <= gen_request_id; + goal_id_r <= goal_id; + data_available_r <= '0'; + done_r <= '1'; + taken_r <= '1'; + return_code_r <= RETCODE_OK; + wait until rising_edge(clk); + done_r <= '0'; + wait until rising_edge(clk); + if (index < test_rrq'length) then + test_rrq(index) := service_info_r.request_id; + end if; + end procedure; + + procedure get_result_response(index_goal : in natural; index_request : in natural) is + begin + wait_on_sig(result_sel); + wait for 1 ps; -- Make sure all signals stable + if (index_goal < test_goals'length and test_goals(index_goal).state = STATUS_SUCCEEDED) then + AffirmIfEqual(RESULT, result_index, test_goals(index_goal).index); + else + AffirmIfEqual(RESULT, result_index, int(MAX_GOALS,result_index'length)); + end if; + result_sel_ack <= '1'; + wait_on_sig(start_r); + wait for 1 ps; -- Make sure all signals stable + AlertIf(opcode_r /= SEND_RESPONSE, "Result service opcode incorrect", FAILURE); + ack_r <= '1'; + wait until rising_edge(clk); + ack_r <= '0'; + wait for 1 ps; -- Make sure all signals stable + if (index_request < test_rrq'length) then + AffirmIfEqual(RESULT, from_request_id(request_id_r), from_request_id(test_rrq(index_request))); + else + AffirmIfEqual(RESULT, from_request_id(request_id_r), from_request_id(service_info_r.request_id)); + end if; + if (index_goal < test_goals'length) then + AffirmIfEqual(RESULT, status_r, test_goals(index_goal).state); + else + AffirmIfEqual(RESULT, status_r, STATUS_UNKNOWN); + end if; + done_r <= '1'; + return_code_r <= RETCODE_OK; + wait until rising_edge(clk); + done_r <= '0'; + result_sel_ack <= '0'; + wait for 1 ps; -- Make sure all signals stable + AlertIf(result_sel = '1', "result_sel still high", FAILURE); + wait until rising_edge(clk); + end procedure; + + procedure get_cancel_request(goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0); stamp : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0)) is + begin + data_available_c <= '1'; + wait_on_sig(start_c); + wait for 1 ps; -- Make sure all signals stable + AlertIf(opcode_c /= TAKE_REQUEST, "Result service opcode incorrect", FAILURE); + ack_c <= '1'; + wait until rising_edge(clk); + ack_c <= '0'; + service_info_c.request_id <= gen_request_id; + goal_info_goal_id_c <= goal_id; + goal_info_stamp_c <= stamp; + data_available_c <= '0'; + done_c <= '1'; + taken_c <= '1'; + return_code_c <= RETCODE_OK; + wait until rising_edge(clk); + done_c <= '0'; + wait until rising_edge(clk); + end procedure; + + procedure user_input_cancel(index : in natural; accept : in boolean) is + begin + wait_on_sig(cancel_request); + AffirmIfEqual(CANCEL, cancel_request_handle, test_goals(index).handle); + cancel_accepted <= '1' when accept else '0'; + cancel_response <= '1'; + wait until rising_edge(clk); + if (accept) then + test_goals(index).state := STATUS_CANCELING; + end if; + cancel_accepted <= '0'; + cancel_response <= '0'; + wait until rising_edge(clk); + end procedure; + + procedure get_cancel_response(ret_code : in std_logic_vector(CDR_INT8_WIDTH-1 downto 0)) is + begin + wait_on_sig(start_c); + AlertIf(opcode_c /= SEND_RESPONSE, "Goal service opcode incorrect", FAILURE); + ack_c <= '1'; + wait until rising_edge(clk); + ack_c <= '0'; + selector_c <= '1'; + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(CANCEL, from_request_id(request_id_c), from_request_id(service_info_c.request_id)); + AffirmIfEqual(CANCEL, cancel_return_code_c, ret_code); + AffirmIfEqual(CANCEL, goals_canceling_len, int(cancel_len,goals_canceling_len'length)); + for i in 0 to cancel_len-1 loop + goals_canceling_addr_u <= int(i,goals_canceling_addr_u'length); + wait_on_sig(goals_canceling_ready); + goals_canceling_ren <= '1'; + wait until rising_edge(clk); + goals_canceling_ren <= '0'; + wait_on_sig(goals_canceling_valid); + AffirmIfEqual(CANCEL, goals_canceling_goal_id_r, test_goals(test_cancel(i)).goal_id); + AffirmIfEqual(CANCEL, goals_canceling_stamp_r, test_goals(test_cancel(i)).stamp); + goals_canceling_ack <= '1'; + wait until rising_edge(clk); + goals_canceling_ack <= '0'; + end loop; + done_c <= '1'; + return_code_c <= RETCODE_OK; + wait until rising_edge(clk); + selector_c <= '0'; + done_c <= '0'; + wait until rising_edge(clk); + end procedure; + + procedure user_op(op : in ROS_ACTION_OPCODE_TYPE) is + begin + start <= '1'; + opcode <= op; + wait_on_sig(ack); + wait until rising_edge(clk); + start <= '0'; + wait_on_sig(done); + end procedure; + + procedure user_fb_op(goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0)) is + begin + start <= '1'; + opcode <= PUBLISH_FEEDBACK; + wait_on_sig(ack); + wait until rising_edge(clk); + start <= '0'; + wait_on_sig(start_fb); + wait for 1 ps; -- Make sure all signals stable + AlertIf(opcode_fb /= PUBLISH, "Goal service opcode incorrect", FAILURE); + ack_fb <= '1'; + wait until rising_edge(clk); + ack_fb <= '0'; + AffirmIfEqual(FEEDBACK, goal_id_fb, goal_id); + done_fb <= '1'; + return_code_fb <= RETCODE_OK; + wait until rising_edge(clk); + AffirmIfEqual(FEEDBACK, done, done_fb); + AffirmIfEqual(FEEDBACK, return_code, return_code_fb); + done_fb <= '0'; + wait until rising_edge(clk); + end procedure; + + -- NOTE: This procedure waits until the idle_sig and mem_done is high for at least + -- two consecutive clock cycles. + procedure wait_on_idle is + variable first : boolean := TRUE; + begin + loop + if (not (idle_sig = '1' and mem_done = '1')) then + wait until idle_sig = '1' and mem_done = '1'; + elsif (not first) then + exit; + end if; + wait until rising_edge(clk); + wait until rising_edge(clk); + first := FALSE; + end loop; + end procedure; + begin + + SetAlertLogName("L0_ros_action_server_test1 - (TIMEOUT 20 sec) - General"); + SetAlertEnable(FAILURE, TRUE); + SetAlertEnable(ERROR, TRUE); + SetAlertEnable(WARNING, TRUE); + SetLogEnable(DEBUG, FALSE); + SetLogEnable(PASSED, FALSE); + SetLogEnable(INFO, TRUE); + RV.InitSeed(RV'instance_name); + GOAL := GetAlertLogID("GOAL", ALERTLOG_BASE_ID); + RESULT := GetAlertLogID("RESULT", ALERTLOG_BASE_ID); + CANCEL := GetAlertLogID("CANCEL", ALERTLOG_BASE_ID); + FEEDBACK := GetAlertLogID("FEEDBACK", ALERTLOG_BASE_ID); + STATUS := GetAlertLogID("STATUS", ALERTLOG_BASE_ID); + USER := GetAlertLogID("USER", ALERTLOG_BASE_ID); + + Log("Initial Reset", INFO); + selector_c <= '0'; + t <= gen_duration(0 sec); + reset <= '1'; + wait until rising_edge(clk); + wait until rising_edge(clk); + reset <= '0'; + + -- TEST: GET_LAST_GOAL [MEMORY EMPTY] + + Log("GET_LAST_GOAL [No Goal]", INFO); + t <= gen_duration(1 sec); + user_op(GET_LAST_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_handle_out, GOAL_HANDLE_UNKNOWN); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: GET_GOAL [INVALID HANDLE] + + Log("GET_GOAL [Invalid Handle]", INFO); + t <= gen_duration(2 sec); + goal_handle_in <= (others => '0'); + user_op(GET_GOAL); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_ACTION_GOAL_HANDLE_INVALID, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: GET_PREVIOUS_GOAL [INVALID HANDLE] + + Log("GET_PREVIOUS_GOAL [Invalid Handle]", INFO); + t <= gen_duration(3 sec); + goal_handle_in <= (others => '0'); + user_op(GET_PREVIOUS_GOAL); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_ACTION_GOAL_HANDLE_INVALID, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: UPDATE_GOAL_STATE [INVALID HANDLE] + + Log("UPDATE_GOAL_STATE [Invalid Handle]", INFO); + t <= gen_duration(4 sec); + goal_handle_in <= (others => '0'); + goal_state_in <= STATUS_EXECUTING; + user_op(UPDATE_GOAL_STATE); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_ACTION_GOAL_HANDLE_INVALID, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: NEW RESULT REQUEST [MEMORY EMPTY] + + Log("New Result Request [Goal 1]", INFO); + t <= gen_duration(5 sec); + get_result_request(2, gen_goal_id); + get_result_response(5,2); + wait_on_idle; + + -- TEST: CANCEL REQUEST [MEMORY EMPTY] + -- TEST: CANCEL REQUEST [NO GOAL ID SET, NO TIME SET, NO GOALS EXIST] + + Log("Cancel Request [ALL]", INFO); + t <= gen_duration(6 sec); + test_cancel := (others => MAX_GOALS); + get_cancel_request((others => '0'), (others => '0')); + get_cancel_response(CancelGoal_package.RR_ERROR_REJECTED); + wait_on_idle; + + -- TEST: NEW GOAL [ACCEPT] + -- TEST: STATUS UPDATE [NEW GOAL] + -- TEST: NEW GOAL [MEMORY EMPTY] + + Log("New Goal [Goal 1, Accept]", INFO); + t <= gen_duration(7 sec); + get_goal_request(0, goal_id_r); + user_input_goal(0, TRUE); + get_goal_response(0); + wait_on_idle; + + -- TEST: NEW GOAL [REJECT] + + Log("New Goal [Goal 2, Reject]", INFO); + t <= gen_duration(8 sec); + get_goal_request(5, gen_goal_id); + user_input_goal(5,FALSE); + get_goal_response(5); + wait_on_idle; + + Log("New Goal [Goal 3, Accept]", INFO); + t <= gen_duration(9 sec); + get_goal_request(1, gen_goal_id); + user_input_goal(1,TRUE); + get_goal_response(1); + wait_on_idle; + + Log("New Goal [Goal 4, Accept]", INFO); + t <= gen_duration(10 sec); + get_goal_request(2, gen_goal_id); + user_input_goal(2,TRUE); + get_goal_response(2); + wait_on_idle; + + Log("New Goal [Goal 5, Accept]", INFO); + t <= gen_duration(11 sec); + get_goal_request(3, gen_goal_id); + user_input_goal(3,TRUE); + get_goal_response(3); + wait_on_idle; + + -- TEST: NEW GOAL [FULL GOAL MEMORY, TIMEOUT/=INFINITE] + + data_available_g <= '1'; + t <= gen_duration(12 sec); + wait until rising_edge(clk); + wait until rising_edge(clk); + AlertIf(start_r = '1', "Goal service operation on full memory", FAILURE); + + -- TEST: UPDATE_GOAL_STATE [VALID STATE] + -- TEST: STATUS UPDATE [GOAL UPDATE] + + Log("UPDATE_GOAL_STATE [Goal 3, Executing]", INFO); + t <= gen_duration(13 sec); + goal_handle_in <= test_goals(1).handle; + goal_state_in <= STATUS_EXECUTING; + user_op(UPDATE_GOAL_STATE); + test_goals(1).state := STATUS_EXECUTING; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + Log("UPDATE_GOAL_STATE [Goal 5, Executing]", INFO); + t <= gen_duration(14 sec); + goal_handle_in <= test_goals(3).handle; + goal_state_in <= STATUS_EXECUTING; + user_op(UPDATE_GOAL_STATE); + test_goals(3).state := STATUS_EXECUTING; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: GET_GOAL + + Log("GET_GOAL [Goal 1]", INFO); + t <= gen_duration(15 sec); + goal_handle_in <= test_goals(0).handle; + user_op(GET_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_id, test_goals(0).goal_id); + AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(0).stamp); + AffirmIfEqual(STATUS, goal_state_out, test_goals(0).state); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: GET_LAST_GOAL [MEMORY NOT EMPTY] + + Log("GET_LAST_GOAL [Goal 5]", INFO); + t <= gen_duration(16 sec); + user_op(GET_LAST_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_handle_out, test_goals(3).handle); + AffirmIfEqual(STATUS, goal_id, test_goals(3).goal_id); + AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(3).stamp); + AffirmIfEqual(STATUS, goal_state_out, test_goals(3).state); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL EXISTS] + + Log("GET_PREVIOUS_GOAL [Goal 4]", INFO); + t <= gen_duration(17 sec); + goal_handle_in <= test_goals(3).handle; + user_op(GET_PREVIOUS_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_handle_out, test_goals(2).handle); + AffirmIfEqual(STATUS, goal_id, test_goals(2).goal_id); + AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(2).stamp); + AffirmIfEqual(STATUS, goal_state_out, test_goals(2).state); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + Log("GET_PREVIOUS_GOAL [Goal 3]", INFO); + t <= gen_duration(18 sec); + goal_handle_in <= test_goals(2).handle; + user_op(GET_PREVIOUS_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_handle_out, test_goals(1).handle); + AffirmIfEqual(STATUS, goal_id, test_goals(1).goal_id); + AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(1).stamp); + AffirmIfEqual(STATUS, goal_state_out, test_goals(1).state); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL DOES NOT EXISTS] + + Log("GET_PREVIOUS_GOAL [No Goal]", INFO); + t <= gen_duration(19 sec); + goal_handle_in <= test_goals(0).handle; + user_op(GET_PREVIOUS_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_handle_out, GOAL_HANDLE_UNKNOWN); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: NEW RESULT REQUEST [GOAL DOES NOT EXIST] + + Log("New Result Request [Unknown Goal]", INFO); + t <= gen_duration(20 sec); + get_result_request(2, gen_goal_id); + get_result_response(5,2); + wait_on_idle; + + -- TEST: NEW RESULT REQUEST [GOAL NOT TERMINAL] + + Log("New Result Request [Goal 1, Store]", INFO); + t <= gen_duration(21 sec); + get_result_request(0, test_goals(0).goal_id); + wait_on_idle; + + -- TEST: STORED RESULT REQUEST [GOAL STATE UPDATES] + + Log("UPDATE_GOAL_STATE [Goal 1, Executing]", INFO); + t <= gen_duration(22 sec); + goal_handle_in <= test_goals(0).handle; + goal_state_in <= STATUS_EXECUTING; + user_op(UPDATE_GOAL_STATE); + test_goals(0).state := STATUS_EXECUTING; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + Log("UPDATE_GOAL_STATE [Goal 5, Succeeded]", INFO); + t <= gen_duration(23 sec); + goal_handle_in <= test_goals(3).handle; + goal_state_in <= STATUS_SUCCEEDED; + user_op(UPDATE_GOAL_STATE); + test_goals(3).state := STATUS_SUCCEEDED; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: NEW RESULT REQUEST [GOAL TERMINAL] + + Log("New Result Request [Goal 5]", INFO); + t <= gen_duration(24 sec); + get_result_request(2, test_goals(3).goal_id); + get_result_response(3,2); + wait_on_idle; + + Log("New Result Request [Goal 3, Store]", INFO); + t <= gen_duration(25 sec); + get_result_request(1, test_goals(1).goal_id); + wait_on_idle; + + -- TEST: NEW RESULT REQUEST [FULL RESULT MEMORY] + + data_available_r <= '1'; + t <= gen_duration(26 sec); + wait until rising_edge(clk); + wait until rising_edge(clk); + AlertIf(start_r = '1', "Result service operation on full memory", FAILURE); + + -- TEST: STORED RESULT REQUEST [GOAL STATE CHANGES TO TERMINAL] + + Log("UPDATE_GOAL_STATE [Goal 1, Aborted]", INFO); + t <= gen_duration(27 sec); + goal_handle_in <= test_goals(0).handle; + goal_state_in <= STATUS_ABORTED; + user_op(UPDATE_GOAL_STATE); + test_goals(0).state := STATUS_ABORTED; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + get_result_response(0,0); + wait_on_idle; + AlertIf(start_r /= '1', "No Result service operation on available memory", FAILURE); + + Log("New Result Request [Goal 4, Store]", INFO); + t <= gen_duration(28 sec); + get_result_request(0, test_goals(2).goal_id); + wait_on_idle; + + -- TEST: UPDATE_GOAL_STATE [INVALID STATE] + + Log("UPDATE_GOAL_STATE [Goal 1, Invalid State Transition]", INFO); + t <= gen_duration(29 sec); + goal_handle_in <= test_goals(0).handle; + goal_state_in <= STATUS_SUCCEEDED; + user_op(UPDATE_GOAL_STATE); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_ACTION_GOAL_EVENT_INVALID, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: CANCEL REQUEST [GOAL ID SET, GOAL DOES NOT EXISTS] + + Log("Cancel Request [Unknown Goal] (ERROR_REJECTED)", INFO); + t <= gen_duration(30 sec); + test_cancel := (others => MAX_GOALS); + get_cancel_request(gen_goal_id, (others => '0')); + get_cancel_response(CancelGoal_package.RR_ERROR_UNKNOWN_GOAL_ID); + wait_on_idle; + + -- TEST: CANCEL REQUEST [GOAL ID SET, GOAL EXISTS, GOAL TERMINAL] + + Log("Cancel Request [Goal 1] (ERROR_GOAL_TERMINATED)", INFO); + t <= gen_duration(31 sec); + test_cancel := (others => MAX_GOALS); + get_cancel_request(test_goals(0).goal_id, (others => '0')); + get_cancel_response(CancelGoal_package.RR_ERROR_GOAL_TERMINATED); + wait_on_idle; + + -- TEST: CANCEL REQUEST [GOAL ID SET, GOAL EXISTS] + -- TEST: STATUS UPDATE [GOAL CANCELED] + + Log("Cancel Request [Goal 3] (Goal 3 Accept)", INFO); + t <= gen_duration(32 sec); + test_cancel := (0 => 1, others => MAX_GOALS); + get_cancel_request(test_goals(1).goal_id, (others => '0')); + user_input_cancel(1,TRUE); + get_cancel_response(CancelGoal_package.RR_ERROR_NONE); + wait_on_idle; + + -- TEST: CANCEL REQUEST [NO GOAL ID SET, NO TIME SET, GOALS EXIST] + + Log("Cancel Request [ALL] (Goal 4 Accept)", INFO); + t <= gen_duration(33 sec); + test_cancel := (0 => 2, 1 => 1, others => MAX_GOALS); + get_cancel_request((others => '0'), (others => '0')); + user_input_cancel(2,TRUE); + get_cancel_response(CancelGoal_package.RR_ERROR_NONE); + wait_on_idle; + + Log("UPDATE_GOAL_STATE [Goal 3, Canceled]", INFO); + t <= gen_duration(34 sec); + goal_handle_in <= test_goals(1).handle; + goal_state_in <= STATUS_CANCELED; + user_op(UPDATE_GOAL_STATE); + test_goals(1).state := STATUS_CANCELED; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + get_result_response(1,1); + wait_on_idle; + + Log("UPDATE_GOAL_STATE [Goal 4, Abort]", INFO); + t <= gen_duration(35 sec); + goal_handle_in <= test_goals(2).handle; + goal_state_in <= STATUS_ABORTED; + user_op(UPDATE_GOAL_STATE); + test_goals(2).state := STATUS_ABORTED; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + get_result_response(2,0); + wait_on_idle; + + -- TEST: EXPIRE GOALS [SINGLE EXPIRED GOAL] + -- TEST: STATUS UPDATE [GOAL REMOVED] + + Log("Expire Goal [Goal 5]", INFO); + t <= gen_duration(43 sec); + test_goal_cnt := test_goal_cnt - 1; + wait_on_idle; + AlertIf(start_g /= '1', "No Goal service operation on available memory", FAILURE); + + + Log("New Goal [Goal 6, Accept]", INFO); + get_goal_request(3, gen_goal_id); + user_input_goal(3,TRUE); + get_goal_response(3); + wait_on_idle; + + -- TEST: CANCEL REQUEST [TIME SET, NO RELEVANT GOALS] + + Log("Cancel Request [Time Set] (NO GOALS)", INFO); + t <= gen_duration(44 sec); + test_cancel := (others => MAX_GOALS); + get_cancel_request((others => '0'), ros_gen_time(40 sec)); + get_cancel_response(CancelGoal_package.RR_ERROR_REJECTED); + wait_on_idle; + + -- TEST: EXPIRE GOALS [MULTIPLE EXPIRED GOALS] + + Log("Expire Goal [Goal 1,3,4]", INFO); + t <= gen_duration(60 sec); + test_goal_cnt := test_goal_cnt - 3; + test_goals(0) := test_goals(3); + wait_on_idle; + + -- TEST: CANCEL REQUEST [TIME SET, RELEVANT GOALS EXIST] + + Log("Cancel Request [Time Set] (Goal 6 Accept)", INFO); + t <= gen_duration(61 sec); + test_cancel := (0 => 0, others => MAX_GOALS); + get_cancel_request((others => '0'), ros_gen_time(43 sec)); + user_input_cancel(0,TRUE); + get_cancel_response(CancelGoal_package.RR_ERROR_NONE); + wait_on_idle; + + Log("UPDATE_GOAL_STATE [Goal 6, Canceled]", INFO); + t <= gen_duration(62 sec); + goal_handle_in <= test_goals(0).handle; + goal_state_in <= STATUS_CANCELED; + user_op(UPDATE_GOAL_STATE); + test_goals(0).state := STATUS_CANCELED; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: EXPIRE GOALS [ALL GOALS EXPIRED] + + Log("Expire Goal [Goal 6]", INFO); + t <= gen_duration(82 sec); + test_goal_cnt := test_goal_cnt - 1; + wait_on_idle; + + -- TEST: RESULT REQUEST MEMORY [EMPTY->FULL->EMPTY->FULL] + + Log("New Goal [Goal 7, Accept]", INFO); + t <= gen_duration(83 sec); + get_goal_request(0, gen_goal_id); + user_input_goal(0, TRUE); + get_goal_response(0); + wait_on_idle; + + Log("New Goal [Goal 8, Accept]", INFO); + t <= gen_duration(84 sec); + get_goal_request(1, gen_goal_id); + user_input_goal(1, TRUE); + get_goal_response(1); + wait_on_idle; + + Log("New Goal [Goal 9, Accept]", INFO); + t <= gen_duration(85 sec); + get_goal_request(2, gen_goal_id); + user_input_goal(2, TRUE); + get_goal_response(2); + wait_on_idle; + + Log("New Goal [Goal 10, Accept]", INFO); + t <= gen_duration(86 sec); + get_goal_request(3, gen_goal_id); + user_input_goal(3, TRUE); + get_goal_response(3); + wait_on_idle; + + -- TEST: GOAL MEMORY [EMPTY->FULL->EMPTY->FULL] + + Log("New Result Request [Goal 8, Store]", INFO); + t <= gen_duration(87 sec); + get_result_request(0, test_goals(1).goal_id); + wait_on_idle; + + Log("New Result Request [Goal 8, Store]", INFO); + t <= gen_duration(88 sec); + get_result_request(1, test_goals(1).goal_id); + wait_on_idle; + + -- TEST: CANCEL REQUEST [GOAL ID SET, TIME SET, GOAL ID IS NOT IN TIME RANGE] + + Log("Cancel Request [Goal Set, Time Set] (Goal 10 Reject, Goal 8 Reject, Goal 7 Accept)", INFO); + t <= gen_duration(89 sec); + test_cancel := (0 => 0, others => MAX_GOALS); + get_cancel_request(test_goals(3).goal_id, ros_gen_time(84 sec)); + user_input_cancel(3,FALSE); + user_input_cancel(1,FALSE); + user_input_cancel(0,TRUE); + get_cancel_response(CancelGoal_package.RR_ERROR_NONE); + wait_on_idle; + + -- TEST: CANCEL REQUEST [GOAL ID SET, TIME SET, GOAL ID IS IN TIME RANGE] + + Log("Cancel Request [Goal Set, Time Set] (Goal 9 Reject, Goal 8 Accept)", INFO); + t <= gen_duration(90 sec); + test_cancel := (0 => 1, 1 => 0, others => MAX_GOALS); + get_cancel_request(test_goals(2).goal_id, ros_gen_time(86 sec)); + user_input_cancel(2,FALSE); + user_input_cancel(3,FALSE); + user_input_cancel(2,FALSE); + user_input_cancel(1,TRUE); + get_cancel_response(CancelGoal_package.RR_ERROR_NONE); + wait_on_idle; + + Log("UPDATE_GOAL_STATE [Goal 7, Abort]", INFO); + t <= gen_duration(91 sec); + goal_handle_in <= test_goals(0).handle; + goal_state_in <= STATUS_ABORTED; + user_op(UPDATE_GOAL_STATE); + test_goals(0).state := STATUS_ABORTED; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: STORED RESULT REQUEST [MULTIPLE REQUESTS, SAME GOAL, GOAL STATE CHANGES TO TERMINAL] + + Log("UPDATE_GOAL_STATE [Goal 8, Succeeded]", INFO); + t <= gen_duration(92 sec); + goal_handle_in <= test_goals(1).handle; + goal_state_in <= STATUS_SUCCEEDED; + user_op(UPDATE_GOAL_STATE); + test_goals(1).state := STATUS_SUCCEEDED; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + get_result_response(1,1); + get_result_response(1,0); + wait_on_idle; + + -- TEST: PUBLISH_FEEDBACK + + Log("FEEDBACK [Goal 9]", INFO); + t <= gen_duration(93 sec); + goal_handle_in <= test_goals(2).handle; + user_fb_op(test_goals(2).goal_id); + + TranscriptOpen(RESULTS_FILE, APPEND_MODE); + SetTranscriptMirror; + ReportAlerts; + TranscriptClose; + std.env.stop; + wait; + end process; + + status_check_prc : process (all) + variable cnt : natural := 0; + begin + if rising_edge(clk) then + if (reset = '1') then + status_stage <= IDLE; + cnt := 0; + else + case(status_stage) is + when IDLE => + if (start_s = '1') then + AlertIf(opcode_s /= PUBLISH, "Status publisher opcode incorrect", FAILURE); + AffirmIfEqual(STATUS, status_list_len, int(test_goal_cnt,status_list_len'length)); + cnt := 0; + if (test_goal_cnt = 0) then + status_stage <= DONE_STATUS; + else + status_stage <= CHECK_1; + end if; + end if; + when CHECK_1 => + if (status_list_ready = '1') then + status_stage <= CHECK_2; + end if; + when CHECK_2 => + if (status_list_valid = '1') then + AffirmIfEqual(STATUS, status_list_goal_info_goal_id_r, test_goals(test_goal_cnt-1-cnt).goal_id); + AffirmIfEqual(STATUS, status_list_goal_info_stamp_r, test_goals(test_goal_cnt-1-cnt).stamp); + AffirmIfEqual(STATUS, status_list_status_r, test_goals(test_goal_cnt-1-cnt).state); + if (cnt = test_goal_cnt-1) then + status_stage <= DONE_STATUS; + else + cnt := cnt + 1; + status_stage <= CHECK_1; + end if; + end if; + when DONE_STATUS => + status_stage <= IDLE; + end case; + end if; + end if; + + -- DEFAULT + status_list_addr_u <= (others => '0'); + status_list_ren <= '0'; + status_list_ack <= '0'; + ack_s <= '0'; + done_s <= '0'; + selector_s <= '0'; + return_code_s <= RETCODE_ERROR; + + case(status_stage) is + when IDLE => + if (start_s = '1') then + ack_s <= '1'; + end if; + when CHECK_1 => + selector_s <= '1'; + status_list_addr_u <= int(cnt,status_list_addr'length); + status_list_ren <= '1'; + when CHECK_2 => + selector_s <= '1'; + if (status_list_valid = '1') then + status_list_ack <= '1'; + end if; + when DONE_STATUS => + done_s <= '1'; + return_code_s <= RETCODE_OK; + when others => + null; + end case; + end process; + + clock_prc : process + begin + clk <= '0'; + wait for TEST_CLOCK_PERIOD/2; + clk <= '1'; + wait for TEST_CLOCK_PERIOD/2; + end process; + + watchdog : process + begin + wait for 1 ms; + Alert("Test timeout", FAILURE); + std.env.stop; + end process; + +end architecture; \ No newline at end of file diff --git a/src/ros2/Tests/Level_0/L0_ros_action_server_test2.vhd b/src/ros2/Tests/Level_0/L0_ros_action_server_test2.vhd new file mode 100644 index 0000000..58f1fef --- /dev/null +++ b/src/ros2/Tests/Level_0/L0_ros_action_server_test2.vhd @@ -0,0 +1,1311 @@ +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.GoalStatus_package.all; +use work.CancelGoal_package; +use work.GoalStatusArray_package; + +-- NOTE: Even though this Test instantiates other components, we only (mis-) use them for the memory they instantiate, and thus this Test only tests the uut device (Level 0) + +-- This testbench tests the General Behavour of ROS Action Server. +-- More specifically following tests are done: +-- TEST: NEW GOAL [MEMORY EMPTY] +-- TEST: NEW GOAL [ACCEPT] +-- TEST: NEW GOAL [REJECT] +-- TEST: NEW GOAL [FULL GOAL MEMORY, TIMEOUT=INFINITE, NO TERMINAL STATE GOAL EXISTS] +-- TEST: NEW GOAL [FULL GOAL MEMORY, TIMEOUT=INFINITE, TERMINAL STATE GOAL EXISTS] +-- TEST: GET_GOAL +-- TEST: GET_GOAL [INVALID HANDLE] +-- TEST: GET_LAST_GOAL [MEMORY NOT EMPTY] +-- TEST: GET_LAST_GOAL [MEMORY EMPTY] +-- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL EXISTS] +-- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL DOES NOT EXISTS] +-- TEST: GET_PREVIOUS_GOAL [INVALID HANDLE] +-- TEST: UPDATE_GOAL_STATE [VALID STATE] +-- TEST: UPDATE_GOAL_STATE [INVALID STATE] +-- TEST: UPDATE_GOAL_STATE [INVALID HANDLE] +-- TEST: PUBLISH_FEEDBACK +-- TEST: NEW RESULT REQUEST [MEMORY EMPTY] +-- TEST: NEW RESULT REQUEST [GOAL DOES NOT EXIST] +-- TEST: NEW RESULT REQUEST [GOAL TERMINAL] +-- TEST: NEW RESULT REQUEST [GOAL NOT TERMINAL] +-- TEST: NEW RESULT REQUEST [FULL RESULT MEMORY] +-- TEST: STORED RESULT REQUEST [GOAL STATE UPDATES] +-- TEST: STORED RESULT REQUEST [GOAL STATE CHANGES TO TERMINAL] +-- TEST: STORED RESULT REQUEST [MULTIPLE REQUESTS, SAME GOAL, GOAL STATE CHANGES TO TERMINAL] +-- TEST: RESULT REQUEST MEMORY [EMPTY->FULL->EMPTY->FULL] +-- TEST: CANCEL REQUEST [MEMORY EMPTY] +-- TEST: CANCEL REQUEST [GOAL ID SET, GOAL EXISTS] +-- TEST: CANCEL REQUEST [GOAL ID SET, GOAL EXISTS, GOAL TERMINAL] +-- TEST: CANCEL REQUEST [GOAL ID SET, GOAL DOES NOT EXISTS] +-- TEST: CANCEL REQUEST [TIME SET, NO RELEVANT GOALS] +-- TEST: CANCEL REQUEST [TIME SET, RELEVANT GOALS EXIST] +-- TEST: CANCEL REQUEST [NO GOAL ID SET, NO TIME SET, GOALS EXIST] +-- TEST: CANCEL REQUEST [NO GOAL ID SET, NO TIME SET, NO GOALS EXIST] +-- TEST: CANCEL REQUEST [GOAL ID SET, TIME SET, GOAL ID IS NOT IN TIME RANGE] +-- TEST: CANCEL REQUEST [GOAL ID SET, TIME SET, GOAL ID IS IN TIME RANGE] +-- TEST: STATUS UPDATE [NEW GOAL] +-- TEST: STATUS UPDATE [GOAL UPDATE] +-- TEST: STATUS UPDATE [GOAL REMOVED] +-- TEST: STATUS UPDATE [GOAL CANCELED] + +entity L0_ros_action_server_test2 is +end entity; + +architecture testbench of L0_ros_action_server_test2 is + + constant MAX_GOALS : natural := 4; + constant MAX_RESULT_REQUESTS : natural := 2; + + type STATUS_CHECK_STAGE_TYPE is (IDLE, CHECK_1, CHECK_2, DONE_STATUS); + + type TEST_GOAL_TYPE is record + goal_id : std_logic_vector(UUID_WIDTH-1 downto 0); + stamp : std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + state : std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + handle : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); + index : std_logic_vector(WORD_WIDTH-1 downto 0); + end record; + type TEST_GOAL_ARRAY_TYPE is array (0 to MAX_GOALS-1) of TEST_GOAL_TYPE; + type TEST_CANCEL_ARRAY_TYPE is array (0 to MAX_GOALS-1) of natural; + + constant ZERO_TEST_GOAL : TEST_GOAL_TYPE := ( + goal_id => (others => '0'), + stamp => (others => '0'), + state => STATUS_UNKNOWN, + handle => GOAL_HANDLE_UNKNOWN, + index => (others => '0') + ); + + type REQUEST_ID_ARRAY_TYPE is array (0 to MAX_RESULT_REQUESTS-1) of REQUEST_ID_TYPE; + + constant ZERO_SAMPLE_INFO : SAMPLE_INFO_TYPE := ( + sample_state => ANY_SAMPLE_STATE, + view_state => ANY_VIEW_STATE, + instance_state => ANY_INSTANCE_STATE, + source_timestamp => TIME_ZERO, + instance_handle => HANDLE_NIL, + publication_handle => HANDLE_NIL, + disposed_generation_count => (others => '0'), + no_writers_generation_count => (others => '0'), + sample_rank => (others => '0'), + generation_rank => (others => '0'), + absolute_generation_rank => (others => '0'), + valid_data => '0' + ); + + signal clk, reset, selector_s, selector_c : std_logic := '0'; + signal t : ROS_TIME_TYPE := ROS_TIME_ZERO; + signal status_stage : STATUS_CHECK_STAGE_TYPE := IDLE; + + signal start_g, ack_g, data_available_g, taken_g, done_g, accepted_g : std_logic := '0'; + signal opcode_g : ROS_SERVICE_OPCODE_TYPE := NOP; + signal service_info_g : SERVICE_INFO_TYPE := EMPTY_SERVICE_INFO; + signal request_id_g : REQUEST_ID_TYPE := EMPTY_REQUEST_ID; + signal return_code_g : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := ROS_RET_OK; + signal goal_id_g : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0'); + signal stamp_g : std_logic_vector(ROS_TIME_WIDTH-1 downto 0) := (others => '0'); + + signal start_r, ack_r, data_available_r, taken_r, done_r, result_sel, result_sel_ack : std_logic := '0'; + signal opcode_r : ROS_SERVICE_OPCODE_TYPE := NOP; + signal service_info_r : SERVICE_INFO_TYPE := EMPTY_SERVICE_INFO; + signal request_id_r : REQUEST_ID_TYPE := EMPTY_REQUEST_ID; + signal return_code_r : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := ROS_RET_OK; + signal result_index : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0'); + signal goal_id_r : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0'); + signal status_r : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0'); + + signal start_c, ack_c, data_available_c, taken_c, done_c : std_logic := '0'; + signal opcode_c : ROS_SERVICE_OPCODE_TYPE := NOP; + signal service_info_c : SERVICE_INFO_TYPE := EMPTY_SERVICE_INFO; + signal request_id_c : REQUEST_ID_TYPE := EMPTY_REQUEST_ID; + signal return_code_c : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := ROS_RET_OK; + signal goal_info_goal_id_c, goals_canceling_goal_id_r, goals_canceling_goal_id_w : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0'); + signal goal_info_stamp_c : std_logic_vector(ROS_TIME_WIDTH-1 downto 0) := (others => '0'); + signal cancel_return_code_c : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0'); + signal goals_canceling_len, goals_canceling_addr, goals_canceling_addr_c, goals_canceling_addr_u : std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0) := (others => '0'); + signal goals_canceling_ready, goals_canceling_ren, goals_canceling_wen, goals_canceling_valid, goals_canceling_ack : std_logic := '0'; + signal goals_canceling_stamp_r, goals_canceling_stamp_w : std_logic_vector(ROS_TIME_WIDTH-1 downto 0) := (others => '0'); + + signal start_fb, ack_fb, done_fb : std_logic := '0'; + signal opcode_fb : ROS_TOPIC_OPCODE_TYPE := NOP; + signal return_code_fb : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0'); + signal goal_id_fb : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0'); + + signal start_s, ack_s, done_s : std_logic := '0'; + signal opcode_s : ROS_TOPIC_OPCODE_TYPE := NOP; + signal return_code_s : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0'); + signal status_list_len, status_list_addr, status_list_addr_s, status_list_addr_u : std_logic_vector(GoalStatusArray_package.STATUS_LIST_ADDR_WIDTH-1 downto 0) := (others => '0'); + signal status_list_ready, status_list_ren, status_list_wen, status_list_valid, status_list_ack : std_logic := '0'; + signal status_list_goal_info_goal_id_r, status_list_goal_info_goal_id_w : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0'); + signal status_list_goal_info_stamp_r, status_list_goal_info_stamp_w : std_logic_vector(ROS_TIME_WIDTH-1 downto 0) := (others => '0'); + signal status_list_status_r, status_list_status_w : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0'); + + signal start, ack, done : std_logic := '0'; + signal opcode : ROS_ACTION_OPCODE_TYPE := NOP; + signal return_code : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0'); + signal goal_handle_in, goal_handle_out : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0) := (others => '0'); + signal goal_state_in, goal_state_out : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0'); + signal goal_id : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0'); + signal goal_result_index : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0'); + signal goal_stamp : ROS_TIME_TYPE := ROS_TIME_ZERO; + signal new_goal_request, new_goal_accepted, new_goal_response, cancel_request, cancel_accepted, cancel_response : std_logic := '0'; + signal new_goal_handle, cancel_request_handle : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0) := (others => '0'); + signal new_goal_result_index : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0'); + + shared variable test_goals : TEST_GOAL_ARRAY_TYPE := (others => ZERO_TEST_GOAL); + shared variable test_goal_cnt : natural := 0; + shared variable GOAL, RESULT, FEEDBACK, STATUS, USER, CANCEL : AlertLogIDType; + + function from_request_id(input : REQUEST_ID_TYPE) return std_logic_vector is + variable ret : std_logic_vector(GUID_WIDTH+63 downto 0); + begin + ret(GUID_WIDTH+63 downto 64) := std_logic_vector(to_unsigned(input.writer_guid)); + ret(63 downto 0) := std_logic_vector(to_unsigned(input.sequence_number)); + return ret; + end function; + +begin + + uut : entity work.ros_action_server(arch) + generic map ( + TIMEOUT_DURATION => ROS_DURATION_INFINITE, + MAX_GOALS => MAX_GOALS, + MAX_RESULT_REQUESTS => 2 + ) + port map ( + clk => clk, + reset => reset, + time => t, + start_g => start_g, + ack_g => ack_g, + opcode_g => opcode_g, + service_info_g => service_info_g, + request_id_g => request_id_g, + data_available_g => data_available_g, + taken_g => taken_g, + done_g => done_g, + return_code_g => return_code_g, + goal_id_g => goal_id_g, + accepted_g => accepted_g, + stamp_g => stamp_g, + start_r => start_r, + ack_r => ack_r, + opcode_r => opcode_r, + service_info_r => service_info_r, + request_id_r => request_id_r, + data_available_r => data_available_r, + taken_r => taken_r, + done_r => done_r, + return_code_r => return_code_r, + result_index => result_index, + result_sel => result_sel, + result_sel_ack => result_sel_ack, + goal_id_r => goal_id_r, + status_r => status_r, + start_c => start_c, + ack_c => ack_c, + opcode_c => opcode_c, + service_info_c => service_info_c, + request_id_c => request_id_c, + data_available_c => data_available_c, + taken_c => taken_c, + done_c => done_c, + return_code_c => return_code_c, + goal_info_goal_id_c => goal_info_goal_id_c, + goal_info_stamp_c => goal_info_stamp_c, + cancel_return_code_c => cancel_return_code_c, + goals_canceling_len_c => goals_canceling_len, + goals_canceling_addr_c => goals_canceling_addr_c, + goals_canceling_ready_c => goals_canceling_ready, + goals_canceling_ren_c => open, + goals_canceling_wen_c => goals_canceling_wen, + goals_canceling_valid_c => goals_canceling_valid, + goals_canceling_ack_c => open, + goals_canceling_goal_id_r_c => (others => '0'), + goals_canceling_goal_id_w_c => goals_canceling_goal_id_w, + goals_canceling_stamp_r_c => (others => '0'), + goals_canceling_stamp_w_c => goals_canceling_stamp_w, + start_fb => start_fb, + opcode_fb => opcode_fb, + ack_fb => ack_fb, + done_fb => done_fb, + return_code_fb => return_code_fb, + goal_id_fb => goal_id_fb, + start_s => start_s, + opcode_s => opcode_s, + ack_s => ack_s, + done_s => done_s, + return_code_s => return_code_s, + status_list_len_s => status_list_len, + status_list_addr_s => status_list_addr_s, + status_list_ready_s => status_list_ready, + status_list_ren_s => open, + status_list_wen_s => status_list_wen, + status_list_valid_s => status_list_valid, + status_list_ack_s => open, + status_list_goal_info_goal_id_r_s => (others => '0'), + status_list_goal_info_goal_id_w_s => status_list_goal_info_goal_id_w, + status_list_goal_info_stamp_r_s => (others => '0'), + status_list_goal_info_stamp_w_s => status_list_goal_info_stamp_w, + status_list_status_r_s => status_list_status_r, + status_list_status_w_s => status_list_status_w, + start => start, + opcode => opcode, + ack => ack, + done => done, + return_code => return_code, + goal_handle_in => goal_handle_in, + goal_handle_out => goal_handle_out, + goal_state_in => goal_state_in, + goal_state_out => goal_state_out, + goal_id => goal_id, + goal_result_index => goal_result_index, + goal_stamp => goal_stamp, + new_goal_request => new_goal_request, + new_goal_handle => new_goal_handle, + new_goal_result_index => new_goal_result_index, + new_goal_accepted => new_goal_accepted, + new_goal_response => new_goal_response, + cancel_request => cancel_request, + cancel_request_handle => cancel_request_handle, + cancel_accepted => cancel_accepted, + cancel_response => cancel_response + ); + + cancel_array : entity work.CancelGoal_ros_srv_server(arch) + port map ( + clk => clk, + reset => reset, + start_r => open, + ack_r => '0', + 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 => '0', + return_code_r => (others => '0'), + valid_in_r => '0', + ready_in_r => open, + data_in_r => (others => '0'), + last_word_in_r => '0', + sample_info_r => ZERO_SAMPLE_INFO, + sample_info_valid_r => '0', + sample_info_ack_r => open, + eoc_r => '0', + status_r => (others => '0'), + start_w => open, + ack_w => '0', + opcode_w => open, + instance_handle_out_w => open, + source_ts_w => open, + max_wait_w => open, + done_w => '0', + return_code_w => (others => '0'), + instance_handle_in_w => HANDLE_NIL, + valid_out_w => open, + ready_out_w => '0', + data_out_w => open, + last_word_out_w => open, + valid_in_w => '0', + ready_in_w => open, + data_in_w => (others => '0'), + last_word_in_w => '0', + status_w => (others => '0'), + start_user => '0', + ack_user => open, + opcode_user => NOP, + service_info_user => open, + request_id_user => EMPTY_REQUEST_ID, + data_available_user => open, + taken_user => open, + goal_info_goal_id => open, + goal_info_stamp => open, + return_code => (others => '0'), + goals_canceling_len => goals_canceling_len, + goals_canceling_addr => goals_canceling_addr, + goals_canceling_ready => goals_canceling_ready, + goals_canceling_ren => goals_canceling_ren, + goals_canceling_wen => goals_canceling_wen, + goals_canceling_valid => goals_canceling_valid, + goals_canceling_ack => goals_canceling_ack, + goals_canceling_goal_id_r => goals_canceling_goal_id_r, + goals_canceling_goal_id_w => goals_canceling_goal_id_w, + goals_canceling_stamp_r => goals_canceling_stamp_r, + goals_canceling_stamp_w => goals_canceling_stamp_w, + done_user => open, + return_code_user => open + ); + + status_array : entity work.GoalStatusArray_ros_pub(arch) + port map ( + clk => clk, + reset => reset, + start_dds => open, + ack_dds => '0', + opcode_dds => open, + instance_handle_out_dds => open, + source_ts_dds => open, + max_wait_dds => open, + done_dds => '0', + return_code_dds => (others => '0'), + instance_handle_in_dds => HANDLE_NIL, + valid_out_dds => open, + ready_out_dds => '0', + data_out_dds => open, + last_word_out_dds => open, + valid_in_dds => '0', + ready_in_dds => open, + data_in_dds => (others => '0'), + last_word_in_dds => '0', + status_dds => (others => '0'), + start_user => '0', + opcode_user => NOP, + ack_user => open, + status_list_len => status_list_len, + status_list_addr => status_list_addr, + status_list_ready => status_list_ready, + status_list_ren => status_list_ren, + status_list_wen => status_list_wen, + status_list_valid => status_list_valid, + status_list_ack => status_list_ack, + status_list_goal_info_goal_id_r => status_list_goal_info_goal_id_r, + status_list_goal_info_goal_id_w => status_list_goal_info_goal_id_w, + status_list_goal_info_stamp_r => status_list_goal_info_stamp_r, + status_list_goal_info_stamp_w => status_list_goal_info_stamp_w, + status_list_status_r => status_list_status_r, + status_list_status_w => status_list_status_w, + done_user => open, + return_code_user => open + ); + + status_list_addr <= status_list_addr_s when (selector_s = '0') else status_list_addr_u; + goals_canceling_addr <= goals_canceling_addr_c when (selector_c = '0') else goals_canceling_addr_u; + + stimulus_prc : process + variable RV : RandomPType; + variable test_rrq : REQUEST_ID_ARRAY_TYPE := (others => EMPTY_REQUEST_ID); + variable test_cancel : TEST_CANCEL_ARRAY_TYPE := (others => 0); + + alias idle_sig is <>; + alias mem_done is <>; + + procedure wait_on_sig(signal sig : std_logic) is + begin + if (sig /= '1') then + wait on sig until sig = '1'; + end if; + end procedure; + + impure function gen_request_id return REQUEST_ID_TYPE is + variable ret : REQUEST_ID_TYPE := EMPTY_REQUEST_ID; + begin + ret.writer_guid := to_guid(RV.RandSlv(GUID_WIDTH)); + ret.sequence_number(0) := unsigned(RV.RandSlv(ret.sequence_number(0)'length)); + ret.sequence_number(1) := unsigned(RV.RandSlv(ret.sequence_number(1)'length)); + return ret; + end function; + + impure function gen_goal_id return std_logic_vector is + begin + return RV.RandSlv(UUID_WIDTH); + end function; + + impure function cancel_len return natural is + variable ret : natural := 0; + begin + for i in 0 to MAX_GOALS-1 loop + if (test_cancel(i) = MAX_GOALS) then + exit; + end if; + ret := ret + 1; + end loop; + return ret; + end function; + + function ros_gen_time(t : time) return std_logic_vector is + variable tmp : ROS_TIME_TYPE; + begin + tmp := gen_duration(t); + return std_logic_vector(to_unsigned(tmp)); + end function; + + procedure get_goal_request(index : in natural; goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0)) is + begin + data_available_g <= '1'; + wait_on_sig(start_g); + wait for 1 ps; -- Make sure all signals stable + AlertIf(opcode_g /= TAKE_REQUEST, "Goal service opcode incorrect", FAILURE); + ack_g <= '1'; + wait until rising_edge(clk); + ack_g <= '0'; + service_info_g.request_id <= gen_request_id; + goal_id_g <= goal_id; + data_available_g <= '0'; + done_g <= '1'; + taken_g <= '1'; + return_code_g <= RETCODE_OK; + wait until rising_edge(clk); + done_g <= '0'; + wait until rising_edge(clk); + if (index < test_goals'length) then + test_goals(index).goal_id := goal_id_g; + end if; + end procedure; + + procedure user_input_goal(index : in natural; accept : in boolean) is + begin + wait_on_sig(new_goal_request); + AlertIf((to_integer(unsigned(new_goal_handle)) / 10) /= to_integer(unsigned(new_goal_result_index)), "Result index out of sync", FAILURE); + if (accept) then + test_goals(index).handle := new_goal_handle; + test_goals(index).index := new_goal_result_index; + end if; + new_goal_accepted <= '1' when accept else '0'; + new_goal_response <= '1'; + wait until rising_edge(clk); + if (accept) then + test_goals(index).stamp := std_logic_vector(to_unsigned(t)); + test_goals(index).state := STATUS_ACCEPTED; + test_goal_cnt := test_goal_cnt + 1; + end if; + new_goal_accepted <= '0'; + new_goal_response <= '0'; + wait until rising_edge(clk); + end procedure; + + procedure get_goal_response(index : in natural) is + begin + wait_on_sig(start_g); + AlertIf(opcode_g /= SEND_RESPONSE, "Goal service opcode incorrect", FAILURE); + ack_g <= '1'; + wait until rising_edge(clk); + ack_g <= '0'; + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(GOAL, from_request_id(request_id_g), from_request_id(service_info_g.request_id)); + if (index < test_goals'length) then + AffirmIfEqual(GOAL, accepted_g, '1'); + else + AffirmIfEqual(GOAL, accepted_g, '0'); + end if; + --AffirmIfEqual(GOAL, stamp_g, test_goals(index).stamp); + AffirmIfEqual(GOAL, stamp_g, std_logic_vector(to_unsigned(t))); + done_g <= '1'; + return_code_g <= RETCODE_OK; + wait until rising_edge(clk); + done_g <= '0'; + wait until rising_edge(clk); + end procedure; + + procedure get_result_request(index : in natural; goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0)) is + begin + data_available_r <= '1'; + wait_on_sig(start_r); + wait for 1 ps; -- Make sure all signals stable + AlertIf(opcode_r /= TAKE_REQUEST, "Result service opcode incorrect", FAILURE); + ack_r <= '1'; + wait until rising_edge(clk); + ack_r <= '0'; + service_info_r.request_id <= gen_request_id; + goal_id_r <= goal_id; + data_available_r <= '0'; + done_r <= '1'; + taken_r <= '1'; + return_code_r <= RETCODE_OK; + wait until rising_edge(clk); + done_r <= '0'; + wait until rising_edge(clk); + if (index < test_rrq'length) then + test_rrq(index) := service_info_r.request_id; + end if; + end procedure; + + procedure get_result_response(index_goal : in natural; index_request : in natural) is + begin + wait_on_sig(result_sel); + wait for 1 ps; -- Make sure all signals stable + if (index_goal < test_goals'length and test_goals(index_goal).state = STATUS_SUCCEEDED) then + AffirmIfEqual(RESULT, result_index, test_goals(index_goal).index); + else + AffirmIfEqual(RESULT, result_index, int(MAX_GOALS,result_index'length)); + end if; + result_sel_ack <= '1'; + wait_on_sig(start_r); + wait for 1 ps; -- Make sure all signals stable + AlertIf(opcode_r /= SEND_RESPONSE, "Result service opcode incorrect", FAILURE); + ack_r <= '1'; + wait until rising_edge(clk); + ack_r <= '0'; + wait for 1 ps; -- Make sure all signals stable + if (index_request < test_rrq'length) then + AffirmIfEqual(RESULT, from_request_id(request_id_r), from_request_id(test_rrq(index_request))); + else + AffirmIfEqual(RESULT, from_request_id(request_id_r), from_request_id(service_info_r.request_id)); + end if; + if (index_goal < test_goals'length) then + AffirmIfEqual(RESULT, status_r, test_goals(index_goal).state); + else + AffirmIfEqual(RESULT, status_r, STATUS_UNKNOWN); + end if; + done_r <= '1'; + return_code_r <= RETCODE_OK; + wait until rising_edge(clk); + done_r <= '0'; + result_sel_ack <= '0'; + wait for 1 ps; -- Make sure all signals stable + AlertIf(result_sel = '1', "result_sel still high", FAILURE); + wait until rising_edge(clk); + end procedure; + + procedure get_cancel_request(goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0); stamp : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0)) is + begin + data_available_c <= '1'; + wait_on_sig(start_c); + wait for 1 ps; -- Make sure all signals stable + AlertIf(opcode_c /= TAKE_REQUEST, "Result service opcode incorrect", FAILURE); + ack_c <= '1'; + wait until rising_edge(clk); + ack_c <= '0'; + service_info_c.request_id <= gen_request_id; + goal_info_goal_id_c <= goal_id; + goal_info_stamp_c <= stamp; + data_available_c <= '0'; + done_c <= '1'; + taken_c <= '1'; + return_code_c <= RETCODE_OK; + wait until rising_edge(clk); + done_c <= '0'; + wait until rising_edge(clk); + end procedure; + + procedure user_input_cancel(index : in natural; accept : in boolean) is + begin + wait_on_sig(cancel_request); + AffirmIfEqual(CANCEL, cancel_request_handle, test_goals(index).handle); + cancel_accepted <= '1' when accept else '0'; + cancel_response <= '1'; + wait until rising_edge(clk); + if (accept) then + test_goals(index).state := STATUS_CANCELING; + end if; + cancel_accepted <= '0'; + cancel_response <= '0'; + wait until rising_edge(clk); + end procedure; + + procedure get_cancel_response(ret_code : in std_logic_vector(CDR_INT8_WIDTH-1 downto 0)) is + begin + wait_on_sig(start_c); + AlertIf(opcode_c /= SEND_RESPONSE, "Goal service opcode incorrect", FAILURE); + ack_c <= '1'; + wait until rising_edge(clk); + ack_c <= '0'; + selector_c <= '1'; + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(CANCEL, from_request_id(request_id_c), from_request_id(service_info_c.request_id)); + AffirmIfEqual(CANCEL, cancel_return_code_c, ret_code); + AffirmIfEqual(CANCEL, goals_canceling_len, int(cancel_len,goals_canceling_len'length)); + for i in 0 to cancel_len-1 loop + goals_canceling_addr_u <= int(i,goals_canceling_addr_u'length); + wait_on_sig(goals_canceling_ready); + goals_canceling_ren <= '1'; + wait until rising_edge(clk); + goals_canceling_ren <= '0'; + wait_on_sig(goals_canceling_valid); + AffirmIfEqual(CANCEL, goals_canceling_goal_id_r, test_goals(test_cancel(i)).goal_id); + AffirmIfEqual(CANCEL, goals_canceling_stamp_r, test_goals(test_cancel(i)).stamp); + goals_canceling_ack <= '1'; + wait until rising_edge(clk); + goals_canceling_ack <= '0'; + end loop; + done_c <= '1'; + return_code_c <= RETCODE_OK; + wait until rising_edge(clk); + selector_c <= '0'; + done_c <= '0'; + wait until rising_edge(clk); + end procedure; + + procedure user_op(op : in ROS_ACTION_OPCODE_TYPE) is + begin + start <= '1'; + opcode <= op; + wait_on_sig(ack); + wait until rising_edge(clk); + start <= '0'; + wait_on_sig(done); + end procedure; + + procedure user_fb_op(goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0)) is + begin + start <= '1'; + opcode <= PUBLISH_FEEDBACK; + wait_on_sig(ack); + wait until rising_edge(clk); + start <= '0'; + wait_on_sig(start_fb); + wait for 1 ps; -- Make sure all signals stable + AlertIf(opcode_fb /= PUBLISH, "Goal service opcode incorrect", FAILURE); + ack_fb <= '1'; + wait until rising_edge(clk); + ack_fb <= '0'; + AffirmIfEqual(FEEDBACK, goal_id_fb, goal_id); + done_fb <= '1'; + return_code_fb <= RETCODE_OK; + wait until rising_edge(clk); + AffirmIfEqual(FEEDBACK, done, done_fb); + AffirmIfEqual(FEEDBACK, return_code, return_code_fb); + done_fb <= '0'; + wait until rising_edge(clk); + end procedure; + + -- NOTE: This procedure waits until the idle_sig and mem_done is high for at least + -- two consecutive clock cycles. + procedure wait_on_idle is + variable first : boolean := TRUE; + begin + loop + if (not (idle_sig = '1' and mem_done = '1')) then + wait until idle_sig = '1' and mem_done = '1'; + elsif (not first) then + exit; + end if; + wait until rising_edge(clk); + wait until rising_edge(clk); + first := FALSE; + end loop; + end procedure; + begin + + SetAlertLogName("L0_ros_action_server_test2 - (TIMEOUT INFINITE) - General"); + SetAlertEnable(FAILURE, TRUE); + SetAlertEnable(ERROR, TRUE); + SetAlertEnable(WARNING, TRUE); + SetLogEnable(DEBUG, FALSE); + SetLogEnable(PASSED, FALSE); + SetLogEnable(INFO, TRUE); + RV.InitSeed(RV'instance_name); + GOAL := GetAlertLogID("GOAL", ALERTLOG_BASE_ID); + RESULT := GetAlertLogID("RESULT", ALERTLOG_BASE_ID); + CANCEL := GetAlertLogID("CANCEL", ALERTLOG_BASE_ID); + FEEDBACK := GetAlertLogID("FEEDBACK", ALERTLOG_BASE_ID); + STATUS := GetAlertLogID("STATUS", ALERTLOG_BASE_ID); + USER := GetAlertLogID("USER", ALERTLOG_BASE_ID); + + Log("Initial Reset", INFO); + selector_c <= '0'; + t <= gen_duration(0 sec); + reset <= '1'; + wait until rising_edge(clk); + wait until rising_edge(clk); + reset <= '0'; + + -- TEST: GET_LAST_GOAL [MEMORY EMPTY] + + Log("GET_LAST_GOAL [No Goal]", INFO); + t <= gen_duration(1 sec); + user_op(GET_LAST_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_handle_out, GOAL_HANDLE_UNKNOWN); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: GET_GOAL [INVALID HANDLE] + + Log("GET_GOAL [Invalid Handle]", INFO); + t <= gen_duration(2 sec); + goal_handle_in <= (others => '0'); + user_op(GET_GOAL); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_ACTION_GOAL_HANDLE_INVALID, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: GET_PREVIOUS_GOAL [INVALID HANDLE] + + Log("GET_PREVIOUS_GOAL [Invalid Handle]", INFO); + t <= gen_duration(3 sec); + goal_handle_in <= (others => '0'); + user_op(GET_PREVIOUS_GOAL); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_ACTION_GOAL_HANDLE_INVALID, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: UPDATE_GOAL_STATE [INVALID HANDLE] + + Log("UPDATE_GOAL_STATE [Invalid Handle]", INFO); + t <= gen_duration(4 sec); + goal_handle_in <= (others => '0'); + goal_state_in <= STATUS_EXECUTING; + user_op(UPDATE_GOAL_STATE); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_ACTION_GOAL_HANDLE_INVALID, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: NEW RESULT REQUEST [MEMORY EMPTY] + + Log("New Result Request [Goal 1]", INFO); + t <= gen_duration(5 sec); + get_result_request(2, gen_goal_id); + get_result_response(5,2); + wait_on_idle; + + -- TEST: CANCEL REQUEST [MEMORY EMPTY] + -- TEST: CANCEL REQUEST [NO GOAL ID SET, NO TIME SET, NO GOALS EXIST] + + Log("Cancel Request [ALL]", INFO); + t <= gen_duration(6 sec); + test_cancel := (others => MAX_GOALS); + get_cancel_request((others => '0'), (others => '0')); + get_cancel_response(CancelGoal_package.RR_ERROR_REJECTED); + wait_on_idle; + + -- TEST: NEW GOAL [ACCEPT] + -- TEST: STATUS UPDATE [NEW GOAL] + -- TEST: NEW GOAL [MEMORY EMPTY] + + Log("New Goal [Goal 1, Accept]", INFO); + t <= gen_duration(7 sec); + get_goal_request(0, goal_id_r); + user_input_goal(0, TRUE); + get_goal_response(0); + wait_on_idle; + + -- TEST: NEW GOAL [REJECT] + + Log("New Goal [Goal 2, Reject]", INFO); + t <= gen_duration(8 sec); + get_goal_request(5, gen_goal_id); + user_input_goal(5,FALSE); + get_goal_response(5); + wait_on_idle; + + Log("New Goal [Goal 3, Accept]", INFO); + t <= gen_duration(9 sec); + get_goal_request(1, gen_goal_id); + user_input_goal(1,TRUE); + get_goal_response(1); + wait_on_idle; + + Log("New Goal [Goal 4, Accept]", INFO); + t <= gen_duration(10 sec); + get_goal_request(2, gen_goal_id); + user_input_goal(2,TRUE); + get_goal_response(2); + wait_on_idle; + + Log("New Goal [Goal 5, Accept]", INFO); + t <= gen_duration(11 sec); + get_goal_request(3, gen_goal_id); + user_input_goal(3,TRUE); + get_goal_response(3); + wait_on_idle; + + -- TEST: NEW GOAL [FULL GOAL MEMORY, TIMEOUT=INFINITE, NO TERMINAL STATE GOAL EXISTS] + + data_available_g <= '1'; + t <= gen_duration(12 sec); + wait until rising_edge(clk); + wait until rising_edge(clk); + AlertIf(start_r = '1', "Goal service operation on full memory", FAILURE); + + -- TEST: UPDATE_GOAL_STATE [VALID STATE] + -- TEST: STATUS UPDATE [GOAL UPDATE] + + Log("UPDATE_GOAL_STATE [Goal 3, Executing]", INFO); + t <= gen_duration(13 sec); + goal_handle_in <= test_goals(1).handle; + goal_state_in <= STATUS_EXECUTING; + user_op(UPDATE_GOAL_STATE); + test_goals(1).state := STATUS_EXECUTING; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + Log("UPDATE_GOAL_STATE [Goal 5, Executing]", INFO); + t <= gen_duration(14 sec); + goal_handle_in <= test_goals(3).handle; + goal_state_in <= STATUS_EXECUTING; + user_op(UPDATE_GOAL_STATE); + test_goals(3).state := STATUS_EXECUTING; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: GET_GOAL + + Log("GET_GOAL [Goal 1]", INFO); + t <= gen_duration(15 sec); + goal_handle_in <= test_goals(0).handle; + user_op(GET_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_id, test_goals(0).goal_id); + AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(0).stamp); + AffirmIfEqual(STATUS, goal_state_out, test_goals(0).state); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: GET_LAST_GOAL [MEMORY NOT EMPTY] + + Log("GET_LAST_GOAL [Goal 5]", INFO); + t <= gen_duration(16 sec); + user_op(GET_LAST_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_handle_out, test_goals(3).handle); + AffirmIfEqual(STATUS, goal_id, test_goals(3).goal_id); + AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(3).stamp); + AffirmIfEqual(STATUS, goal_state_out, test_goals(3).state); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL EXISTS] + + Log("GET_PREVIOUS_GOAL [Goal 4]", INFO); + t <= gen_duration(17 sec); + goal_handle_in <= test_goals(3).handle; + user_op(GET_PREVIOUS_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_handle_out, test_goals(2).handle); + AffirmIfEqual(STATUS, goal_id, test_goals(2).goal_id); + AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(2).stamp); + AffirmIfEqual(STATUS, goal_state_out, test_goals(2).state); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + Log("GET_PREVIOUS_GOAL [Goal 3]", INFO); + t <= gen_duration(18 sec); + goal_handle_in <= test_goals(2).handle; + user_op(GET_PREVIOUS_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_handle_out, test_goals(1).handle); + AffirmIfEqual(STATUS, goal_id, test_goals(1).goal_id); + AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(1).stamp); + AffirmIfEqual(STATUS, goal_state_out, test_goals(1).state); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL DOES NOT EXISTS] + + Log("GET_PREVIOUS_GOAL [No Goal]", INFO); + t <= gen_duration(19 sec); + goal_handle_in <= test_goals(0).handle; + user_op(GET_PREVIOUS_GOAL); + wait for 1 ps; -- Make sure all signals stable + AffirmIfEqual(STATUS, goal_handle_out, GOAL_HANDLE_UNKNOWN); + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: NEW RESULT REQUEST [GOAL DOES NOT EXIST] + + Log("New Result Request [Unknown Goal]", INFO); + t <= gen_duration(20 sec); + get_result_request(2, gen_goal_id); + get_result_response(5,2); + wait_on_idle; + + -- TEST: NEW RESULT REQUEST [GOAL NOT TERMINAL] + + Log("New Result Request [Goal 1, Store]", INFO); + t <= gen_duration(21 sec); + get_result_request(0, test_goals(0).goal_id); + wait_on_idle; + + -- TEST: STORED RESULT REQUEST [GOAL STATE UPDATES] + + Log("UPDATE_GOAL_STATE [Goal 1, Executing]", INFO); + t <= gen_duration(22 sec); + goal_handle_in <= test_goals(0).handle; + goal_state_in <= STATUS_EXECUTING; + user_op(UPDATE_GOAL_STATE); + test_goals(0).state := STATUS_EXECUTING; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + data_available_g <= '0'; + + Log("UPDATE_GOAL_STATE [Goal 5, Succeeded]", INFO); + t <= gen_duration(23 sec); + goal_handle_in <= test_goals(3).handle; + goal_state_in <= STATUS_SUCCEEDED; + user_op(UPDATE_GOAL_STATE); + test_goals(3).state := STATUS_SUCCEEDED; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: NEW RESULT REQUEST [GOAL TERMINAL] + + Log("New Result Request [Goal 5]", INFO); + t <= gen_duration(24 sec); + get_result_request(2, test_goals(3).goal_id); + get_result_response(3,2); + wait_on_idle; + + Log("New Result Request [Goal 3, Store]", INFO); + t <= gen_duration(25 sec); + get_result_request(1, test_goals(1).goal_id); + wait_on_idle; + + -- TEST: NEW RESULT REQUEST [FULL RESULT MEMORY] + + data_available_r <= '1'; + t <= gen_duration(26 sec); + wait until rising_edge(clk); + wait until rising_edge(clk); + AlertIf(start_r = '1', "Result service operation on full memory", FAILURE); + + -- TEST: STORED RESULT REQUEST [GOAL STATE CHANGES TO TERMINAL] + + Log("UPDATE_GOAL_STATE [Goal 1, Aborted]", INFO); + t <= gen_duration(27 sec); + goal_handle_in <= test_goals(0).handle; + goal_state_in <= STATUS_ABORTED; + user_op(UPDATE_GOAL_STATE); + test_goals(0).state := STATUS_ABORTED; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + get_result_response(0,0); + wait_on_idle; + AlertIf(start_r /= '1', "No Result service operation on available memory", FAILURE); + + Log("New Result Request [Goal 4, Store]", INFO); + t <= gen_duration(28 sec); + get_result_request(0, test_goals(2).goal_id); + wait_on_idle; + + -- TEST: UPDATE_GOAL_STATE [INVALID STATE] + + Log("UPDATE_GOAL_STATE [Goal 1, Invalid State Transition]", INFO); + t <= gen_duration(29 sec); + goal_handle_in <= test_goals(0).handle; + goal_state_in <= STATUS_SUCCEEDED; + user_op(UPDATE_GOAL_STATE); + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_ACTION_GOAL_EVENT_INVALID, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: CANCEL REQUEST [GOAL ID SET, GOAL DOES NOT EXISTS] + + Log("Cancel Request [Unknown Goal] (ERROR_REJECTED)", INFO); + t <= gen_duration(30 sec); + test_cancel := (others => MAX_GOALS); + get_cancel_request(gen_goal_id, (others => '0')); + get_cancel_response(CancelGoal_package.RR_ERROR_UNKNOWN_GOAL_ID); + wait_on_idle; + + -- TEST: CANCEL REQUEST [GOAL ID SET, GOAL EXISTS, GOAL TERMINAL] + + Log("Cancel Request [Goal 1] (ERROR_GOAL_TERMINATED)", INFO); + t <= gen_duration(31 sec); + test_cancel := (others => MAX_GOALS); + get_cancel_request(test_goals(0).goal_id, (others => '0')); + get_cancel_response(CancelGoal_package.RR_ERROR_GOAL_TERMINATED); + wait_on_idle; + + -- TEST: CANCEL REQUEST [GOAL ID SET, GOAL EXISTS] + -- TEST: STATUS UPDATE [GOAL CANCELED] + + Log("Cancel Request [Goal 3] (Goal 3 Accept)", INFO); + t <= gen_duration(32 sec); + test_cancel := (0 => 1, others => MAX_GOALS); + get_cancel_request(test_goals(1).goal_id, (others => '0')); + user_input_cancel(1,TRUE); + get_cancel_response(CancelGoal_package.RR_ERROR_NONE); + wait_on_idle; + + -- TEST: CANCEL REQUEST [NO GOAL ID SET, NO TIME SET, GOALS EXIST] + + Log("Cancel Request [ALL] (Goal 4 Accept)", INFO); + t <= gen_duration(33 sec); + test_cancel := (0 => 2, 1 => 1, others => MAX_GOALS); + get_cancel_request((others => '0'), (others => '0')); + user_input_cancel(2,TRUE); + get_cancel_response(CancelGoal_package.RR_ERROR_NONE); + wait_on_idle; + + Log("UPDATE_GOAL_STATE [Goal 3, Canceled]", INFO); + t <= gen_duration(34 sec); + goal_handle_in <= test_goals(1).handle; + goal_state_in <= STATUS_CANCELED; + user_op(UPDATE_GOAL_STATE); + test_goals(1).state := STATUS_CANCELED; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + get_result_response(1,1); + wait_on_idle; + + Log("UPDATE_GOAL_STATE [Goal 4, Abort]", INFO); + t <= gen_duration(35 sec); + goal_handle_in <= test_goals(2).handle; + goal_state_in <= STATUS_ABORTED; + user_op(UPDATE_GOAL_STATE); + test_goals(2).state := STATUS_ABORTED; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + get_result_response(2,0); + wait_on_idle; + + -- TEST: NEW GOAL [FULL GOAL MEMORY, TIMEOUT=INFINITE, TERMINAL STATE GOAL EXISTS] + -- TEST: STATUS UPDATE [GOAL REMOVED] + + Log("New Goal [Goal 6, Accept]", INFO); + t <= gen_duration(43 sec); + test_goals(0) := test_goals(1); + test_goals(1) := test_goals(2); + test_goals(2) := test_goals(3); + test_goal_cnt := test_goal_cnt - 1; + get_goal_request(3, gen_goal_id); + user_input_goal(3,TRUE); + get_goal_response(3); + wait_on_idle; + + -- TEST: CANCEL REQUEST [TIME SET, NO RELEVANT GOALS] + + Log("Cancel Request [Time Set] (NO GOALS)", INFO); + t <= gen_duration(44 sec); + test_cancel := (others => MAX_GOALS); + get_cancel_request((others => '0'), ros_gen_time(40 sec)); + get_cancel_response(CancelGoal_package.RR_ERROR_REJECTED); + wait_on_idle; + + -- TEST: CANCEL REQUEST [TIME SET, RELEVANT GOALS EXIST] + + Log("Cancel Request [Time Set] (Goal 6 Accept)", INFO); + t <= gen_duration(61 sec); + test_cancel := (0 => 3, others => MAX_GOALS); + get_cancel_request((others => '0'), ros_gen_time(43 sec)); + user_input_cancel(3,TRUE); + get_cancel_response(CancelGoal_package.RR_ERROR_NONE); + wait_on_idle; + + Log("UPDATE_GOAL_STATE [Goal 6, Canceled]", INFO); + t <= gen_duration(62 sec); + goal_handle_in <= test_goals(3).handle; + goal_state_in <= STATUS_CANCELED; + user_op(UPDATE_GOAL_STATE); + test_goals(3).state := STATUS_CANCELED; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: RESULT REQUEST MEMORY [EMPTY->FULL->EMPTY->FULL] + + Log("New Goal [Goal 7, Accept]", INFO); + t <= gen_duration(83 sec); + test_goals(0) := test_goals(1); + test_goals(1) := test_goals(2); + test_goals(2) := test_goals(3); + test_goal_cnt := test_goal_cnt - 1; + get_goal_request(3, gen_goal_id); + user_input_goal(3, TRUE); + get_goal_response(3); + wait_on_idle; + + Log("New Goal [Goal 8, Accept]", INFO); + t <= gen_duration(84 sec); + test_goals(0) := test_goals(1); + test_goals(1) := test_goals(2); + test_goals(2) := test_goals(3); + test_goal_cnt := test_goal_cnt - 1; + get_goal_request(3, gen_goal_id); + user_input_goal(3, TRUE); + get_goal_response(3); + wait_on_idle; + + Log("New Goal [Goal 9, Accept]", INFO); + t <= gen_duration(85 sec); + test_goals(0) := test_goals(1); + test_goals(1) := test_goals(2); + test_goals(2) := test_goals(3); + test_goal_cnt := test_goal_cnt - 1; + get_goal_request(3, gen_goal_id); + user_input_goal(3, TRUE); + get_goal_response(3); + wait_on_idle; + + Log("New Goal [Goal 10, Accept]", INFO); + t <= gen_duration(86 sec); + test_goals(0) := test_goals(1); + test_goals(1) := test_goals(2); + test_goals(2) := test_goals(3); + test_goal_cnt := test_goal_cnt - 1; + get_goal_request(3, gen_goal_id); + user_input_goal(3, TRUE); + get_goal_response(3); + wait_on_idle; + + -- TEST: GOAL MEMORY [EMPTY->FULL->EMPTY->FULL] + + Log("New Result Request [Goal 8, Store]", INFO); + t <= gen_duration(87 sec); + get_result_request(0, test_goals(1).goal_id); + wait_on_idle; + + Log("New Result Request [Goal 8, Store]", INFO); + t <= gen_duration(88 sec); + get_result_request(1, test_goals(1).goal_id); + wait_on_idle; + + -- TEST: CANCEL REQUEST [GOAL ID SET, TIME SET, GOAL ID IS NOT IN TIME RANGE] + + Log("Cancel Request [Goal Set, Time Set] (Goal 10 Reject, Goal 8 Reject, Goal 7 Accept)", INFO); + t <= gen_duration(89 sec); + test_cancel := (0 => 0, others => MAX_GOALS); + get_cancel_request(test_goals(3).goal_id, ros_gen_time(84 sec)); + user_input_cancel(3,FALSE); + user_input_cancel(1,FALSE); + user_input_cancel(0,TRUE); + get_cancel_response(CancelGoal_package.RR_ERROR_NONE); + wait_on_idle; + + -- TEST: CANCEL REQUEST [GOAL ID SET, TIME SET, GOAL ID IS IN TIME RANGE] + + Log("Cancel Request [Goal Set, Time Set] (Goal 9 Reject, Goal 8 Accept)", INFO); + t <= gen_duration(90 sec); + test_cancel := (0 => 1, 1 => 0, others => MAX_GOALS); + get_cancel_request(test_goals(2).goal_id, ros_gen_time(86 sec)); + user_input_cancel(2,FALSE); + user_input_cancel(3,FALSE); + user_input_cancel(2,FALSE); + user_input_cancel(1,TRUE); + get_cancel_response(CancelGoal_package.RR_ERROR_NONE); + wait_on_idle; + + Log("UPDATE_GOAL_STATE [Goal 7, Abort]", INFO); + t <= gen_duration(91 sec); + goal_handle_in <= test_goals(0).handle; + goal_state_in <= STATUS_ABORTED; + user_op(UPDATE_GOAL_STATE); + test_goals(0).state := STATUS_ABORTED; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + wait_on_idle; + + -- TEST: STORED RESULT REQUEST [MULTIPLE REQUESTS, SAME GOAL, GOAL STATE CHANGES TO TERMINAL] + + Log("UPDATE_GOAL_STATE [Goal 8, Succeeded]", INFO); + t <= gen_duration(92 sec); + goal_handle_in <= test_goals(1).handle; + goal_state_in <= STATUS_SUCCEEDED; + user_op(UPDATE_GOAL_STATE); + test_goals(1).state := STATUS_SUCCEEDED; + wait for 1 ps; -- Make sure all signals stable + AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE); + get_result_response(1,1); + get_result_response(1,0); + wait_on_idle; + + -- TEST: PUBLISH_FEEDBACK + + Log("FEEDBACK [Goal 9]", INFO); + t <= gen_duration(93 sec); + goal_handle_in <= test_goals(2).handle; + user_fb_op(test_goals(2).goal_id); + + TranscriptOpen(RESULTS_FILE, APPEND_MODE); + SetTranscriptMirror; + ReportAlerts; + TranscriptClose; + std.env.stop; + wait; + end process; + + status_check_prc : process (all) + variable cnt : natural := 0; + begin + if rising_edge(clk) then + if (reset = '1') then + status_stage <= IDLE; + cnt := 0; + else + case(status_stage) is + when IDLE => + if (start_s = '1') then + AlertIf(opcode_s /= PUBLISH, "Status publisher opcode incorrect", FAILURE); + AffirmIfEqual(STATUS, status_list_len, int(test_goal_cnt,status_list_len'length)); + cnt := 0; + if (test_goal_cnt = 0) then + status_stage <= DONE_STATUS; + else + status_stage <= CHECK_1; + end if; + end if; + when CHECK_1 => + if (status_list_ready = '1') then + status_stage <= CHECK_2; + end if; + when CHECK_2 => + if (status_list_valid = '1') then + AffirmIfEqual(STATUS, status_list_goal_info_goal_id_r, test_goals(test_goal_cnt-1-cnt).goal_id); + AffirmIfEqual(STATUS, status_list_goal_info_stamp_r, test_goals(test_goal_cnt-1-cnt).stamp); + AffirmIfEqual(STATUS, status_list_status_r, test_goals(test_goal_cnt-1-cnt).state); + if (cnt = test_goal_cnt-1) then + status_stage <= DONE_STATUS; + else + cnt := cnt + 1; + status_stage <= CHECK_1; + end if; + end if; + when DONE_STATUS => + status_stage <= IDLE; + end case; + end if; + end if; + + -- DEFAULT + status_list_addr_u <= (others => '0'); + status_list_ren <= '0'; + status_list_ack <= '0'; + ack_s <= '0'; + done_s <= '0'; + selector_s <= '0'; + return_code_s <= RETCODE_ERROR; + + case(status_stage) is + when IDLE => + if (start_s = '1') then + ack_s <= '1'; + end if; + when CHECK_1 => + selector_s <= '1'; + status_list_addr_u <= int(cnt,status_list_addr'length); + status_list_ren <= '1'; + when CHECK_2 => + selector_s <= '1'; + if (status_list_valid = '1') then + status_list_ack <= '1'; + end if; + when DONE_STATUS => + done_s <= '1'; + return_code_s <= RETCODE_OK; + when others => + null; + end case; + end process; + + clock_prc : process + begin + clk <= '0'; + wait for TEST_CLOCK_PERIOD/2; + clk <= '1'; + wait for TEST_CLOCK_PERIOD/2; + end process; + + watchdog : process + begin + wait for 1 ms; + Alert("Test timeout", FAILURE); + std.env.stop; + end process; + +end architecture; \ No newline at end of file diff --git a/src/ros2/Tests/Level_0/ros_action_server_tests.txt b/src/ros2/Tests/Level_0/ros_action_server_tests.txt new file mode 100644 index 0000000..6b76a33 --- /dev/null +++ b/src/ros2/Tests/Level_0/ros_action_server_tests.txt @@ -0,0 +1,50 @@ +-- TEST: NEW GOAL [MEMORY EMPTY] +-- TEST: NEW GOAL [ACCEPT] +-- TEST: NEW GOAL [REJECT] +-- TEST: NEW GOAL [FULL GOAL MEMORY, TIMEOUT=INFINITE, NO TERMINAL STATE GOAL EXISTS] +-- TEST: NEW GOAL [FULL GOAL MEMORY, TIMEOUT=INFINITE, TERMINAL STATE GOAL EXISTS] +-- TEST: NEW GOAL [FULL GOAL MEMORY, TIMEOUT/=INFINITE] + +-- TEST: GET_GOAL +-- TEST: GET_GOAL [INVALID HANDLE] +-- TEST: GET_LAST_GOAL [MEMORY NOT EMPTY] +-- TEST: GET_LAST_GOAL [MEMORY EMPTY] +-- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL EXISTS] +-- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL DOES NOT EXISTS] +-- TEST: GET_PREVIOUS_GOAL [INVALID HANDLE] +-- TEST: UPDATE_GOAL_STATE [VALID STATE] +-- TEST: UPDATE_GOAL_STATE [INVALID STATE] +-- TEST: UPDATE_GOAL_STATE [INVALID HANDLE] +-- TEST: PUBLISH_FEEDBACK + +-- TEST: NEW RESULT REQUEST [MEMORY EMPTY] +-- TEST: NEW RESULT REQUEST [GOAL DOES NOT EXIST] +-- TEST: NEW RESULT REQUEST [GOAL TERMINAL] +-- TEST: NEW RESULT REQUEST [GOAL NOT TERMINAL] +-- TEST: NEW RESULT REQUEST [FULL RESULT MEMORY] +-- TEST: STORED RESULT REQUEST [GOAL STATE UPDATES] +-- TEST: STORED RESULT REQUEST [GOAL STATE CHANGES TO TERMINAL] +-- TEST: STORED RESULT REQUEST [MULTIPLE REQUESTS, SAME GOAL, GOAL STATE CHANGES TO TERMINAL] + +-- TEST: EXPIRE GOALS [SINGLE EXPIRED GOAL] +-- TEST: EXPIRE GOALS [MULTIPLE EXPIRED GOALS] +-- TEST: EXPIRE GOALS [ALL GOALS EXPIRED] + +-- TEST: GOAL MEMORY [EMPTY->FULL->EMPTY->FULL] +-- TEST: RESULT REQUEST MEMORY [EMPTY->FULL->EMPTY->FULL] + +-- TEST: CANCEL REQUEST [MEMORY EMPTY] +-- TEST: CANCEL REQUEST [GOAL ID SET, GOAL EXISTS] +-- TEST: CANCEL REQUEST [GOAL ID SET, GOAL EXISTS, GOAL TERMINAL] +-- TEST: CANCEL REQUEST [GOAL ID SET, GOAL DOES NOT EXISTS] +-- TEST: CANCEL REQUEST [TIME SET, NO RELEVANT GOALS] +-- TEST: CANCEL REQUEST [TIME SET, RELEVANT GOALS EXIST] +-- TEST: CANCEL REQUEST [NO GOAL ID SET, NO TIME SET, GOALS EXIST] +-- TEST: CANCEL REQUEST [NO GOAL ID SET, NO TIME SET, NO GOALS EXIST] +-- TEST: CANCEL REQUEST [GOAL ID SET, TIME SET, GOAL ID IS NOT IN TIME RANGE] +-- TEST: CANCEL REQUEST [GOAL ID SET, TIME SET, GOAL ID IS IN TIME RANGE] + +-- TEST: STATUS UPDATE [NEW GOAL] +-- TEST: STATUS UPDATE [GOAL UPDATE] +-- TEST: STATUS UPDATE [GOAL REMOVED] +-- TEST: STATUS UPDATE [GOAL CANCELED] \ No newline at end of file diff --git a/src/ros2/Tests/ros_testbench.pro b/src/ros2/Tests/ros_testbench.pro index fbc5726..753226d 100644 --- a/src/ros2/Tests/ros_testbench.pro +++ b/src/ros2/Tests/ros_testbench.pro @@ -118,6 +118,9 @@ analyze ../example_interfaces/Fibonacci_ros_action_result_srv_server.vhd analyze ../example_interfaces/Fibonacci_ros_action_result_srv_client.vhd analyze Level_1/L1_Fibonacci_ros_action_result_srv_test1.vhd analyze Level_1/L1_Fibonacci_ros_action_result_srv_test2.vhd +analyze ../ros_action_server.vhd +analyze Level_0/L0_ros_action_server_test1.vhd +analyze Level_0/L0_ros_action_server_test2.vhd simulate L1_AddTwoInts_ros_srv_test1 @@ -132,4 +135,6 @@ simulate L1_Fibonacci_ros_action_feedback_test2 simulate L1_Fibonacci_ros_action_goal_srv_test1 simulate L1_Fibonacci_ros_action_goal_srv_test2 simulate L1_Fibonacci_ros_action_result_srv_test1 -simulate L1_Fibonacci_ros_action_result_srv_test2 \ No newline at end of file +simulate L1_Fibonacci_ros_action_result_srv_test2 +simulate L0_ros_action_server_test1 +simulate L0_ros_action_server_test2 \ No newline at end of file diff --git a/src/ros2/ros_action_server.vhd b/src/ros2/ros_action_server.vhd new file mode 100644 index 0000000..c5acbad --- /dev/null +++ b/src/ros2/ros_action_server.vhd @@ -0,0 +1,3327 @@ +-- 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.math_pkg.all; +use work.rtps_package.all; +use work.rtps_config_package.all; +use work.ros_package.all; +use work.Fibonacci_package.all; +use work.GoalStatus_package; +use work.CancelGoal_package; +use work.GoalStatusArray_package; + +-- NOTE: New Goals coming from the Goal Service are not checked against their goal id, which may lead two multiple goals with same goal id being stored in memory. +-- But this is expected behaviour and not the responsibility of the action server. +-- NOTE: If both the goal id and time of a cancel request are set, and the goal with the set goal id falls also in the requested time range, a double user cancel +-- prompt on the same goal may happen if the first prompt is rejected. + +entity ros_action_server is + generic ( + TIMEOUT_DURATION : ROS_DURATION_TYPE; + MAX_GOALS : natural; + MAX_RESULT_REQUESTS : natural; + ENABLE_FEEDBACK : std_logic := '1' + ); + port ( + -- SYSTEM + clk : in std_logic; + reset : in std_logic; + time : in ROS_TIME_TYPE; + -- *GOAL SERVICE* + start_g : out std_logic; + ack_g : in std_logic; + opcode_g : out ROS_SERVICE_OPCODE_TYPE; + service_info_g : in SERVICE_INFO_TYPE; + request_id_g : out REQUEST_ID_TYPE; + data_available_g : in std_logic; + taken_g : in std_logic; + done_g : in std_logic; + return_code_g : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + -- REQUEST + goal_id_g : in std_logic_vector(UUID_WIDTH-1 downto 0); + -- RESPONSE + accepted_g : out std_logic; + stamp_g : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + -- *RESULT SERVICE* + start_r : out std_logic; + ack_r : in std_logic; + opcode_r : out ROS_SERVICE_OPCODE_TYPE; + service_info_r : in SERVICE_INFO_TYPE; + request_id_r : out REQUEST_ID_TYPE; + data_available_r : in std_logic; + taken_r : in std_logic; + done_r : in std_logic; + return_code_r : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + result_index : out std_logic_vector(WORD_WIDTH-1 downto 0); + result_sel : out std_logic; + result_sel_ack : in std_logic; + -- REQUEST + goal_id_r : in std_logic_vector(UUID_WIDTH-1 downto 0); + -- RESPONSE + status_r : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + -- *CANCEL SERVICE* + start_c : out std_logic; + ack_c : in std_logic; + opcode_c : out ROS_SERVICE_OPCODE_TYPE; + service_info_c : in SERVICE_INFO_TYPE; + request_id_c : out REQUEST_ID_TYPE; + data_available_c : in std_logic; + taken_c : in std_logic; + done_c : in std_logic; + return_code_c : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + -- REQUEST + goal_info_goal_id_c : in std_logic_vector(UUID_WIDTH-1 downto 0); + goal_info_stamp_c : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + -- RESPONSE + cancel_return_code_c : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + goals_canceling_len_c : out std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0); + goals_canceling_addr_c : out std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0); + goals_canceling_ready_c : in std_logic; + goals_canceling_ren_c : out std_logic; + goals_canceling_wen_c : out std_logic; + goals_canceling_valid_c : in std_logic; + goals_canceling_ack_c : out std_logic; + goals_canceling_goal_id_r_c : in std_logic_vector(UUID_WIDTH-1 downto 0); + goals_canceling_goal_id_w_c : out std_logic_vector(UUID_WIDTH-1 downto 0); + goals_canceling_stamp_r_c : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + goals_canceling_stamp_w_c : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + -- *FEEDBACK TOPIC* + start_fb : out std_logic; + opcode_fb : out ROS_TOPIC_OPCODE_TYPE; + ack_fb : in std_logic; + done_fb : in std_logic; + return_code_fb : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + goal_id_fb : out std_logic_vector(UUID_WIDTH-1 downto 0); + -- *STATUS TOPIC* + start_s : out std_logic; + opcode_s : out ROS_TOPIC_OPCODE_TYPE; + ack_s : in std_logic; + done_s : in std_logic; + return_code_s : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + status_list_len_s : out std_logic_vector(GoalStatusArray_package.STATUS_LIST_ADDR_WIDTH-1 downto 0); + status_list_addr_s : out std_logic_vector(GoalStatusArray_package.STATUS_LIST_ADDR_WIDTH-1 downto 0); + status_list_ready_s : in std_logic; + status_list_ren_s : out std_logic; + status_list_wen_s : out std_logic; + status_list_valid_s : in std_logic; + status_list_ack_s : out std_logic; + status_list_goal_info_goal_id_r_s : in std_logic_vector(UUID_WIDTH-1 downto 0); + status_list_goal_info_goal_id_w_s : out std_logic_vector(UUID_WIDTH-1 downto 0); + status_list_goal_info_stamp_r_s : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + status_list_goal_info_stamp_w_s : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + status_list_status_r_s : in std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + status_list_status_w_s : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + -- *USER* + start : in std_logic; + opcode : in ROS_ACTION_OPCODE_TYPE; + ack : out std_logic; + done : out std_logic; + return_code : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + goal_handle_in : in std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); + goal_handle_out : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); + goal_state_in : in std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + goal_state_out : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + goal_id : out std_logic_vector(UUID_WIDTH-1 downto 0); + goal_result_index : out std_logic_vector(WORD_WIDTH-1 downto 0); + goal_stamp : out ROS_TIME_TYPE; + -- GOAL + new_goal_request : out std_logic; + new_goal_handle : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); + new_goal_result_index : out std_logic_vector(WORD_WIDTH-1 downto 0); + new_goal_accepted : in std_logic; + new_goal_response : in std_logic; + -- CANCEL + cancel_request : out std_logic; + cancel_request_handle : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); + cancel_accepted : in std_logic; + cancel_response : in std_logic + ); +end entity; + +architecture arch of ros_action_server is + + --*****CONSTANT DECLARATION***** + -- *GOAL MEMORY* + -- 4-Byte Word Size of a Goal Entry in Memory + function gen_goal_frame_size(TIMEOUT_DURATION : ROS_DURATION_TYPE) return natural is + begin + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then + return 12; + else + return 10; + end if; + end function; + constant GOAL_FRAME_SIZE : natural := gen_goal_frame_size(TIMEOUT_DURATION); + -- Goal Memory Size in 4-Byte Words + constant GOAL_MEMORY_SIZE : natural := MAX_GOALS * GOAL_FRAME_SIZE; + -- Goal Memory Address Width + constant GOAL_MEMORY_ADDR_WIDTH : natural := log2c(GOAL_MEMORY_SIZE); + -- Highest Goal Memory Address + constant GOAL_MEMORY_MAX_ADDRESS : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0) := to_unsigned(GOAL_MEMORY_SIZE-1, GOAL_MEMORY_ADDR_WIDTH); + -- Highest Goal Frame Address + constant MAX_GOAL_ADDRESS : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0) := GOAL_MEMORY_MAX_ADDRESS - GOAL_FRAME_SIZE + 1; + -- Address pointing to the beginning of the first Goal Data Frame + constant FIRST_GOAL_ADDRESS : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0'); + -- *RESULT REQUEST MEMORY* + -- 4-Byte Word Size of a Result Request Entry in Memory + constant RRQ_FRAME_SIZE : natural := 9; + -- Result Request Memory Size in 4-Byte Words + constant RRQ_MEMORY_SIZE : natural := MAX_RESULT_REQUESTS * RRQ_FRAME_SIZE; + -- Result Request Memory Address Width + constant RRQ_MEMORY_ADDR_WIDTH : natural := log2c(RRQ_MEMORY_SIZE); + -- Highest Result Request Memory Address + constant RRQ_MEMORY_MAX_ADDRESS : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0) := to_unsigned(RRQ_MEMORY_SIZE-1, RRQ_MEMORY_ADDR_WIDTH); + -- Highest Result Request Frame Address + constant MAX_RRQ_ADDRESS : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0) := RRQ_MEMORY_MAX_ADDRESS - RRQ_FRAME_SIZE + 1; + -- Address pointing to the beginning of the first Result Request Data Frame + constant FIRST_RRQ_ADDRESS : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0'); + + -- *GOAL MEMORY FRAME FIELD OFFSETS* + -- 4-Byte Word Offsets to Beginning of Respective Fields in the Goal Memory Frame + constant GMF_STATE_OFFSET : natural := 0; + constant GMF_GOAL_ID_OFFSET : natural := 1; + constant GMF_STAMP_OFFSET : natural := 5; + constant GMF_DEADLINE_OFFSET : natural := 7; + function gen_gmf_result_index_offset(TIMEOUT_DURATION : ROS_DURATION_TYPE) return natural is + variable ret : natural := 0; + begin + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then + ret := GMF_DEADLINE_OFFSET + 2; + else + ret := GMF_DEADLINE_OFFSET; + end if; + return ret; + end function; + constant GMF_RESULT_INDEX_OFFSET : natural := gen_gmf_result_index_offset(TIMEOUT_DURATION); + constant GMF_NEXT_ADDR_OFFSET : natural := GMF_RESULT_INDEX_OFFSET + 1; + constant GMF_PREV_ADDR_OFFSET : natural := GMF_NEXT_ADDR_OFFSET + 1; + -- *RESULT REQUEST MEMORY FRAME FIELD OFFSETS* + -- 4-Byte Word Offsets to Beginning of Respective Fields in the Result Request Memory Frame + constant RMF_GOAL_HANDLE_OFFSET : natural := 0; + constant RMF_REQUEST_ID_OFFSET : natural := 1; + constant RMF_NEXT_ADDR_OFFSET : natural := 7; + constant RMF_PREV_ADDR_OFFSET : natural := 8; + + -- *GOAL MEMORY FRAME FIELD FLAGS* + -- Flags mapping to the respective Goal Memory Frame Fields + constant GMF_FLAG_WIDTH : natural := 7; + constant GMF_STATE_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (0 => '1', others => '0'); + constant GMF_GOAL_ID_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (1 => '1', others => '0'); + constant GMF_STAMP_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (2 => '1', others => '0'); + constant GMF_DEADLINE_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (3 => '1', others => '0'); + constant GMF_RESULT_INDEX_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (4 => '1', others => '0'); + constant GMF_NEXT_ADDR_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (5 => '1', others => '0'); + constant GMF_PREV_ADDR_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (6 => '1', others => '0'); + + --*****TYPE DECLARATION***** + -- *Cancel Process Search Types* + -- SEARCH_GOAL_ID Search goal list for a specific goal id + -- SEARCH_STAMP Search goal list for first goal that was accepted at or before a provided timestamp + -- SEARCH_NONE No search criteria. All goals are valid candites (Applies from current list position) + type SEARCH_TYPE is (SEARCH_GOAL_ID, SEARCH_STAMP, SEARCH_NONE); + -- *Instance Memory Opcodes* + -- OPCODE DESCRIPTION + -- INSERT Insert goal into memory + -- GET Get data of goal pointed by "GOAL_DATA_TYPE.addr" according to "GOAL_DATA_TYPE.field_flags". + -- Already fetched data of the goal is not modified. + -- GET_NEXT Get goal data of next goal (from the goal pointed by "GOAL_DATA_TYPE.addr") according to "GOAL_DATA_TYPE.field_flags". + -- Set "GOAL_DATA_TYPE.addr" to address of goal or GOAL_MEMORY_MAX_ADDRESS if no other goal in Memory. + -- GET_PREV Get goal data of previous goal (from the goal pointed by "GOAL_DATA_TYPE.addr") according to "GOAL_DATA_TYPE.field_flags". + -- Set "GOAL_DATA_TYPE.addr" to address of goal or GOAL_MEMORY_MAX_ADDRESS if no other goal in Memory. + -- UPDATE Update goal data pointed by "GOAL_DATA_TYPE.addr" according to "GOAL_DATA_TYPE.field_flags" + -- REMOVE Remove goal pointed by "GOAL_DATA_TYPE.addr" + type MEM_OPCODE_TYPE is (NOP, INSERT, GET, GET_NEXT, GET_PREV, UPDATE, REMOVE); + type STAGE_TYPE is (IDLE,GET_S_DATA,PUSH_STATUS,PUBLISH_STATUS,WAIT_FOR_S_PUBLISHER,TRANSITION_STATE,GET_R_DATA,CHECK_REQUESTS,GET_REQUEST_ID,REMOVE_REQUEST,WAIT_FOR_U_MEM,RETURN_USER,PUBLISH_FEEDBACK,WAIT_FOR_G_DATA, + WAIT_FOR_G_USER,ADD_GOAL,SEND_G_RESPONSE,WAIT_FOR_G_RET,WAIT_FOR_R_DATA,RESULT_CHECK,STORE_RRQ,SEND_R_RESPONSE,WAIT_FOR_R_RET,WAIT_FOR_C_DATA,GET_C_DATA,CANCEL_CHECK,WAIT_FOR_C_USER,CANCEL_GOAL,ADD_CANCEL,SEND_C_RESPONSE, + WAIT_FOR_C_RET,CHECK_TIMEOUT,REMOVE_OLDEST,RESET_MEMORY); + type MEM_STAGE_TYPE is (IDLE,INSERT,GET,GET_NEXT,GET_PREV,UPDATE,GET_RESULT_INDEX,REMOVE,RESET_MEMORY); + type GOAL_DATA_TYPE is record + addr : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); + goal_id : GUID_TYPE; + stamp : ROS_TIME_TYPE; + deadline : ROS_DURATION_TYPE; + res_ind : std_logic_vector(WORD_WIDTH-1 downto 0); + state : std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + field_flags : std_logic_vector(0 to GMF_FLAG_WIDTH-1); + end record; + constant ZERO_GOAL_DATA : GOAL_DATA_TYPE := ( + addr => (others => '0'), + goal_id => GUID_UNKNOWN, + stamp => ROS_TIME_ZERO, + deadline => ROS_DURATION_INFINITE, + res_ind => (others => '0'), + state => GoalStatus_package.STATUS_UNKNOWN, + field_flags => (others => '0') + ); + + --*****SIGNAL DECLARATION***** + -- *GOAL MEMORY CONNECTION SIGNALS* + signal mem_addr : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); + signal mem_read : std_logic; + signal mem_read_data, mem_write_data : std_logic_vector(WORD_WIDTH-1 downto 0); + signal mem_ready_in, mem_valid_in : std_logic; + signal mem_ready_out, mem_valid_out : std_logic; + signal mem_abort_read : std_logic; + -- *RESULT REQUEST MEMORY CONNECTION SIGNALS* + signal rrq_addr : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0); + signal rrq_read : std_logic; + signal rrq_read_data, rrq_write_data : std_logic_vector(WORD_WIDTH-1 downto 0); + signal rrq_ready_in, rrq_valid_in : std_logic; + signal rrq_ready_out, rrq_valid_out : std_logic; + signal rrq_abort_read : std_logic; + + -- *MAIN PROCESS* + -- FSM state + signal stage, stage_next : STAGE_TYPE; + -- Request memory operation + signal mem_start : std_logic; + -- Opcode of goal memory operation (valid only if mem_start is high) + signal mem_opcode : MEM_OPCODE_TYPE; + -- Signal used to pass data to Goal Memory Process + signal mem_r : GOAL_DATA_TYPE; + -- Goal memory data latch + signal mem_data_r, mem_data_r_next : GOAL_DATA_TYPE; + -- Head of occupied result requests list + signal rrq_occupied_head, rrq_occupied_head_next : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0); + -- Head of empty result requests list + signal rrq_empty_head, rrq_empty_head_next : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0); + -- General purpose counter + signal cnt, cnt_next : natural range 0 to 11; + -- Pointer to current relevant Result Request Memory Frame + signal rrq_addr_base, rrq_addr_base_next : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0); + -- Result Request Memory Frame pointer latch + signal rrq_addr_latch, rrq_addr_latch_next : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0); + -- Count of stored goals that are in a terminal state + signal terminal_cnt, terminal_cnt_next : natural range 0 to MAX_GOALS; + -- User return code latch + signal return_code_latch, return_code_latch_next : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + -- Toggle latch used to trigger stored result request search when a goal is updated to a terminal state + signal trigger_result, trigger_result_next : std_logic; + -- Result service Request ID latch + signal request_id_latch, request_id_latch_next : REQUEST_ID_TYPE; + -- Time of next goal expiration dealine + signal check_time, check_time_next : ROS_TIME_TYPE; + -- Temporal time latch (used in time addition calculation) + signal time_latch, time_latch_next : ROS_TIME_TYPE; + -- Goal handle latch + signal goal_handle_latch, goal_handle_latch_next : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); + -- Goal state latch + signal goal_state_latch, goal_state_latch_next :std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + -- Goal timestamp latch + signal g_stamp, g_stamp_next : ROS_TIME_TYPE; + -- Goal accept latch + signal g_accept, g_accept_next : std_logic; + -- Signals the kind of goal cancel search the FSM is doing (see SEARCH_TYPE definition) + signal search_type_c, search_type_c_next : SEARCH_TYPE; + -- Index of goals_canceling sequence + signal goals_canceling_cnt, goals_canceling_cnt_next : natural range 0 to CancelGoal_package.RR_GOALS_CANCELING_MAX_DEPTH-1; + -- Cancel response return code latch + signal cancel_ret_code, cancel_ret_code_next : std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + -- Toggle latch that latches status update requests from other processes + signal trigger_status, trigger_status_next : std_logic; + -- Index of statusl list sequence + signal status_list_cnt, status_list_cnt_next : natural range 0 to MAX_GOALS; + -- Test signal used for testbench synchronisation + signal idle_sig : std_logic; + + -- *GOAL MEMORY PROCESS* + -- Memory process FSM state + signal mem_stage, mem_stage_next : MEM_STAGE_TYPE; + -- Head of occupied goal list + signal mem_occupied_head, mem_occupied_head_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); + -- Tail of occupied goal list + signal mem_occupied_tail, mem_occupied_tail_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); + -- Head of empty goal list + signal mem_empty_head, mem_empty_head_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); + -- Tail of empty goal list + signal mem_empty_tail, mem_empty_tail_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); + -- General purpose counter + signal mem_cnt, mem_cnt_next : natural range 0 to 19; + -- Pointer to currently relevant goal memory frame + signal mem_addr_base, mem_addr_base_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); + -- Goal memory fram pointer latch + signal mem_addr_latch, mem_addr_latch_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); + -- Result index of current empty head goal + signal empty_head_res_ind, empty_head_res_ind_next : unsigned(WORD_WIDTH-1 downto 0); + -- Latch for data from processes + signal mem_latch_data, mem_latch_data_next : GOAL_DATA_TYPE; + -- Latch for goal memory data + signal mem_data, mem_data_next : GOAL_DATA_TYPE; + -- Signals end of memory operation (And that the memory process is in the IDLE state) + signal mem_done : std_logic; + + -- *FUNCTION DECLARATION* + -- Returns true if the state is terminal + function is_terminal (state : std_logic_vector) return boolean is + begin + assert (state'length = CDR_INT8_WIDTH) severity FAILURE; + + case (state) is + when GoalStatus_package.STATUS_SUCCEEDED => + return TRUE; + when GoalStatus_package.STATUS_CANCELED => + return TRUE; + when GoalStatus_package.STATUS_ABORTED => + return TRUE; + when others => + return FALSE; + end case; + end function; + +begin + + goal_mem_ctrl_inst : entity work.mem_ctrl(arch) + generic map ( + ADDR_WIDTH => GOAL_MEMORY_ADDR_WIDTH, + DATA_WIDTH => WORD_WIDTH, + MEMORY_DEPTH => GOAL_MEMORY_SIZE, + MAX_BURST_LENGTH => GOAL_FRAME_SIZE + ) + port map ( + clk => clk, + reset => reset or mem_abort_read, + addr => std_logic_vector(mem_addr), + read => mem_read, + ready_in => mem_ready_in, + valid_in => mem_valid_in, + data_in => mem_write_data, + ready_out => mem_ready_out, + valid_out => mem_valid_out, + data_out => mem_read_data + ); + + rrq_mem_ctrl_inst : entity work.mem_ctrl(arch) + generic map ( + ADDR_WIDTH => RRQ_MEMORY_ADDR_WIDTH, + DATA_WIDTH => WORD_WIDTH, + MEMORY_DEPTH => RRQ_MEMORY_SIZE, + MAX_BURST_LENGTH => RRQ_FRAME_SIZE + ) + port map ( + clk => clk, + reset => reset or rrq_abort_read, + addr => std_logic_vector(rrq_addr), + read => rrq_read, + ready_in => rrq_ready_in, + valid_in => rrq_valid_in, + data_in => rrq_write_data, + ready_out => rrq_ready_out, + valid_out => rrq_valid_out, + data_out => rrq_read_data + ); + + result_index <= mem_data_r.res_ind; + status_r <= mem_data_r.state; + request_id_r <= request_id_latch; + goal_id_fb <= std_logic_vector(to_unsigned(mem_data_r.goal_id)); + goal_id <= std_logic_vector(to_unsigned(mem_data_r.goal_id)); + goal_stamp <= mem_data_r.stamp; + goal_state_out <= mem_data_r.state; + goal_result_index <= mem_data_r.res_ind; + new_goal_handle <= std_logic_vector(resize(mem_empty_head,GOAL_HANDLE_WIDTH)); + new_goal_result_index <= std_logic_vector(empty_head_res_ind); + request_id_g <= service_info_g.request_id; + accepted_g <= g_accept; + stamp_g <= std_logic_vector(to_unsigned(g_stamp)); + request_id_c <= service_info_c.request_id; + cancel_return_code_c <= cancel_ret_code; + goals_canceling_len_c <= std_logic_vector(to_unsigned(goals_canceling_cnt,goals_canceling_len_c'length)); + goals_canceling_addr_c <= std_logic_vector(to_unsigned(goals_canceling_cnt,CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH)); + goals_canceling_goal_id_w_c <= std_logic_vector(to_unsigned(mem_data_r.goal_id)); + goals_canceling_stamp_w_c <= std_logic_vector(to_unsigned(mem_data_r.stamp)); + status_list_len_s <= std_logic_vector(to_unsigned(status_list_cnt,status_list_len_s'length)); + status_list_addr_s <= std_logic_vector(to_unsigned(status_list_cnt,GoalStatusArray_package.STATUS_LIST_ADDR_WIDTH)); + status_list_goal_info_goal_id_w_s <= std_logic_vector(to_unsigned(mem_data_r.goal_id)); + status_list_goal_info_stamp_w_s <= std_logic_vector(to_unsigned(mem_data_r.stamp)); + status_list_status_w_s <= mem_data_r.state; + + + -- *Main State Machine* + -- STATE DESCRIPTION + -- IDLE Idle state + -- GET_S_DATA Get goal data from memory + -- PUSH_STATUS Add goal data to status list + -- PUBLISH_STATUS Publish status list + -- WAIT_FOR_S_PUBLISHER Wait for status list publisher + -- TRANSITION_STATE Update goal state + -- GET_R_DATA Get goal data required for result response + -- CHECK_REQUESTS Search if updated goal is in stored result requests + -- GET_REQUEST_ID Get data from stored result request + -- REMOVE_REQUEST Remove stored result request + -- WAIT_FOR_U_MEM Wait for goal data from memory + -- RETURN_USER User return + -- PUBLISH_FEEDBACK Publish feedback + -- WAIT_FOR_G_DATA Wait goal service request + -- WAIT_FOR_G_USER Wait for user input + -- ADD_GOAL Add goal to memory + -- SEND_G_RESPONSE Send goal service response + -- WAIT_FOR_G_RET Wait for goal service response + -- WAIT_FOR_R_DATA Wait for result service request + -- RESULT_CHECK Search for result request goal + -- STORE_RRQ Store result request in memory + -- SEND_R_RESPONSE Send result service response + -- WAIT_FOR_R_RET Wait for result service response + -- WAIT_FOR_C_DATA Wait for cancel service request + -- GET_C_DATA Get goal data from memory + -- CANCEL_CHECK Search fot goal cancel targets + -- WAIT_FOR_C_USER Wait for user input + -- CANCEL_GOAL Update goal state to CANCELING + -- ADD_CANCEL Store canceling goal for later response + -- SEND_C_RESPONSE Send cancel service response + -- WAIT_FOR_C_RET Wait for cancel service response + -- CHECK_TIMEOUT Check and remove expired goals + -- REMOVE_OLDEST Remove oldest goal in terminal state + -- RESET_MEMORY Reset result request memory to empty state + main_prc : process(all) + begin + -- DEFAULT + stage_next <= stage; + cnt_next <= cnt; + mem_data_r_next <= mem_data_r; + terminal_cnt_next <= terminal_cnt; + return_code_latch_next <= return_code_latch; + trigger_result_next <= trigger_result; + rrq_addr_base_next <= rrq_addr_base; + rrq_addr_latch_next <= rrq_addr_latch; + rrq_empty_head_next <= rrq_empty_head; + rrq_occupied_head_next <= rrq_occupied_head; + request_id_latch_next <= request_id_latch; + time_latch_next <= time_latch; + check_time_next <= check_time; + goal_handle_latch_next <= goal_handle_latch; + goal_state_latch_next <= goal_state_latch; + g_stamp_next <= g_stamp; + g_accept_next <= g_accept; + trigger_status_next <= trigger_status; + status_list_cnt_next <= status_list_cnt; + search_type_c_next <= search_type_c; + goals_canceling_cnt_next <= goals_canceling_cnt; + cancel_ret_code_next <= cancel_ret_code; + -- DEFAULT Unregistered + idle_sig <= '0'; + ack <= '0'; + done <= '0'; + start_fb <= '0'; + rrq_abort_read <= '0'; + result_sel <= '0'; + mem_start <= '0'; + mem_opcode <= NOP; + start_r <= '0'; + opcode_r <= NOP; + opcode_fb <= NOP; + mem_r <= ZERO_GOAL_DATA; + return_code <= ROS_RET_OK; + goal_handle_out <= GOAL_HANDLE_UNKNOWN; + start_g <= '0'; + opcode_g <= NOP; + new_goal_request <= '0'; + start_s <= '0'; + opcode_s <= NOP; + status_list_ren_s <= '0'; + status_list_wen_s <= '0'; + status_list_ack_s <= '0'; + start_c <= '0'; + opcode_c <= NOP; + cancel_request <= '0'; + cancel_request_handle <= GOAL_HANDLE_UNKNOWN; + goals_canceling_ren_c <= '0'; + goals_canceling_wen_c <= '0'; + goals_canceling_ack_c <= '0'; + rrq_addr <= (others => '0'); + rrq_read <= '0'; + rrq_valid_in <= '0'; + rrq_write_data <= (others => '0'); + rrq_ready_out <= '0'; + + + + case (stage) is + when IDLE => + idle_sig <= '1'; + + if (trigger_status = '1') then + -- NOTE: We wait for memory to make sure the mem_occupied_head is set after initial insert + if (mem_done = '1') then + -- Trigger Status Resetter + trigger_status_next <= '0'; + + -- Reset + status_list_cnt_next <= 0; + + if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then + stage_next <= PUBLISH_STATUS; + else + stage_next <= GET_S_DATA; + cnt_next <= 0; + end if; + end if; + elsif (start = '1') then + case (opcode) is + when GET_GOAL => + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= GET; + mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG; + mem_r.addr <= resize(unsigned(goal_handle_in),GOAL_MEMORY_ADDR_WIDTH); + + ack <= '1'; + stage_next <= WAIT_FOR_U_MEM; + return_code_latch_next <= ROS_RET_OK; + end if; + when GET_LAST_GOAL => + -- No Goals Available + if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then + mem_data_r_next.addr <= GOAL_MEMORY_MAX_ADDRESS; + ack <= '1'; + return_code_latch_next <= ROS_RET_OK; + stage_next <= RETURN_USER; + else + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= GET; + mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG; + mem_r.addr <= mem_occupied_head; + + ack <= '1'; + stage_next <= WAIT_FOR_U_MEM; + return_code_latch_next <= ROS_RET_OK; + end if; + end if; + when GET_PREVIOUS_GOAL => + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= GET_NEXT; + mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG; + mem_r.addr <= resize(unsigned(goal_handle_in),GOAL_MEMORY_ADDR_WIDTH); + + ack <= '1'; + stage_next <= WAIT_FOR_U_MEM; + return_code_latch_next <= ROS_RET_OK; + end if; + when UPDATE_GOAL_STATE => + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= GET; + mem_r.field_flags <= GMF_STATE_FLAG; + mem_r.addr <= resize(unsigned(goal_handle_in),GOAL_MEMORY_ADDR_WIDTH); + + -- Latch Input Data + goal_handle_latch_next <= goal_handle_in; + goal_state_latch_next <= goal_state_in; + + ack <= '1'; + stage_next <= TRANSITION_STATE; + cnt_next <= 0; + end if; + when PUBLISH_FEEDBACK => + -- SYNTHESIS GUARD + if (ENABLE_FEEDBACK = '1') then + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= GET; + mem_r.field_flags <= GMF_GOAL_ID_FLAG; + mem_r.addr <= resize(unsigned(goal_handle_in),GOAL_MEMORY_ADDR_WIDTH); + + ack <= '1'; + stage_next <= PUBLISH_FEEDBACK; + cnt_next <= 0; + end if; + else + ack <= '1'; + return_code_latch_next <= ROS_RET_UNSUPPORTED; + stage_next <= RETURN_USER; + end if; + when others => + ack <= '1'; + return_code_latch_next <= ROS_RET_UNSUPPORTED; + stage_next <= RETURN_USER; + end case; + elsif (TIMEOUT_DURATION = ROS_DURATION_INFINITE and mem_empty_head = GOAL_MEMORY_MAX_ADDRESS and terminal_cnt /= 0 and data_available_g = '1') then + assert (mem_occupied_tail /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + mem_data_r_next.addr <= mem_occupied_tail; + stage_next <= REMOVE_OLDEST; + cnt_next <= 0; + elsif (data_available_g = '1' and mem_empty_head /= GOAL_MEMORY_MAX_ADDRESS) then + start_g <= '1'; + opcode_g <= TAKE_REQUEST; + if (ack_g = '1') then + stage_next <= WAIT_FOR_G_DATA; + end if; + elsif (data_available_r = '1' and rrq_empty_head /= RRQ_MEMORY_MAX_ADDRESS) then + start_r <= '1'; + opcode_r <= TAKE_REQUEST; + + if (ack_r = '1') then + stage_next <= WAIT_FOR_R_DATA; + end if; + elsif (data_available_c = '1') then + start_c <= '1'; + opcode_c <= TAKE_REQUEST; + if (ack_c = '1') then + stage_next <= WAIT_FOR_C_DATA; + + --Reset + goals_canceling_cnt_next <= 0; + end if; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and time >= check_time) then + assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + -- Reset + check_time_next <= ROS_TIME_INFINITE; + + mem_data_r_next.addr <= mem_occupied_head; + stage_next <= CHECK_TIMEOUT; + cnt_next <= 0; + end if; + when GET_S_DATA => + case (cnt) is + -- GET FIRST + when 0 => + assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= GET; + mem_r.addr <= mem_occupied_head; + mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG; + + cnt_next <= cnt + 2; + end if; + -- GET NEXT + when 1 => + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= GET_NEXT; + mem_r.addr <= mem_data_r.addr; + mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG; + + cnt_next <= cnt + 1; + end if; + -- WAIT FROM MEM + when 2 => + -- Wait for DATA + if (mem_done = '1') then + mem_data_r_next <= mem_data; + + -- No more Goals + if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then + stage_next <= PUBLISH_STATUS; + else + stage_next <= PUSH_STATUS; + end if; + end if; + when others => + null; + end case; + when PUSH_STATUS => + status_list_wen_s <= '1'; + + if (status_list_ready_s = '1') then + status_list_cnt_next <= status_list_cnt + 1; + + if (status_list_cnt = GoalStatusArray_package.STATUS_LIST_MAX_DEPTH-1) then + stage_next <= PUBLISH_STATUS; + else + stage_next <= GET_S_DATA; + cnt_next <= 1; -- GET_NEXT + end if; + end if; + when PUBLISH_STATUS => + start_s <= '1'; + opcode_s <= PUBLISH; + + if (ack_s = '1') then + stage_next <= WAIT_FOR_S_PUBLISHER; + end if; + when WAIT_FOR_S_PUBLISHER => + if (done_s = '1') then + -- TODO: Propagate Error? + stage_next <= IDLE; + end if; + when TRANSITION_STATE => + case (cnt) is + -- CHECK Goal State + when 0 => + -- Wait for Memory + if (mem_done = '1') then + assert check_mask(mem_data.field_flags, GMF_STATE_FLAG) severity FAILURE; + + case (mem_data.state) is + when GoalStatus_package.STATUS_ACCEPTED => + case (goal_state_latch) is + when GoalStatus_package.STATUS_EXECUTING => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_in)) then + cnt_next <= cnt + 1; + else + cnt_next <= cnt + 3; -- UPDATE Goal State + end if; + when others => + stage_next <= RETURN_USER; + return_code_latch_next <= ROS_RET_ACTION_GOAL_EVENT_INVALID; + end case; + when GoalStatus_package.STATUS_EXECUTING => + case (goal_state_latch) is + when GoalStatus_package.STATUS_SUCCEEDED => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_in)) then + cnt_next <= cnt + 1; + else + cnt_next <= cnt + 3; -- UPDATE Goal State + end if; + when GoalStatus_package.STATUS_ABORTED => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_in)) then + cnt_next <= cnt + 1; + else + cnt_next <= cnt + 3; -- UPDATE Goal State + end if; + when others => + stage_next <= RETURN_USER; + return_code_latch_next <= ROS_RET_ACTION_GOAL_EVENT_INVALID; + end case; + when GoalStatus_package.STATUS_CANCELING => + case (goal_state_latch) is + when GoalStatus_package.STATUS_SUCCEEDED => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_in)) then + cnt_next <= cnt + 1; + else + cnt_next <= cnt + 3; -- UPDATE Goal State + end if; + when GoalStatus_package.STATUS_ABORTED => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_in)) then + cnt_next <= cnt + 1; + else + cnt_next <= cnt + 3; -- UPDATE Goal State + end if; + when GoalStatus_package.STATUS_CANCELED => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_in)) then + cnt_next <= cnt + 1; + else + cnt_next <= cnt + 3; -- UPDATE Goal State + end if; + when others => + stage_next <= RETURN_USER; + return_code_latch_next <= ROS_RET_ACTION_GOAL_EVENT_INVALID; + end case; + when GoalStatus_package.STATUS_UNKNOWN => + -- NOTE: All goals in the empty list have state set to STATUS_UNKNOWN + stage_next <= RETURN_USER; + return_code_latch_next <= ROS_RET_ACTION_GOAL_HANDLE_INVALID; + when others => + stage_next <= RETURN_USER; + return_code_latch_next <= ROS_RET_ACTION_GOAL_EVENT_INVALID; + end case; + end if; + -- Time Addition 1/2 + when 1 => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then + time_latch_next.sec <= std_logic_vector(unsigned(time.sec) + unsigned(TIMEOUT_DURATION.sec)); + time_latch_next.nanosec <= std_logic_vector(unsigned(time.nanosec) + unsigned(TIMEOUT_DURATION.nanosec)); + cnt_next <= cnt + 1; + end if; + -- Time Addition 2/2 + when 2 => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then + if (unsigned(time_latch.nanosec) >= to_unsigned(10**9,time_latch.nanosec'length)) then + time_latch_next.sec <= std_logic_vector(unsigned(time_latch.sec) + 1); + time_latch_next.nanosec <= std_logic_vector(unsigned(time_latch.nanosec) - to_unsigned(10**9,time_latch.nanosec'length)); + end if; + cnt_next <= cnt + 1; + end if; + -- UPDATE Goal State + when 3 => + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= UPDATE; + mem_r.field_flags <= GMF_STATE_FLAG; + mem_r.addr <= resize(unsigned(goal_handle_latch),GOAL_MEMORY_ADDR_WIDTH); + mem_r.state <= goal_state_latch; + + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_in)) then + mem_r.field_flags <= GMF_STATE_FLAG or GMF_DEADLINE_FLAG; + mem_r.deadline <= time_latch; + + -- Update Checktime + if (time_latch < check_time) then + check_time_next <= time_latch; + end if; + end if; + + if (is_terminal(goal_state_in)) then + trigger_result_next <= '1'; + -- Update Terminal Goal Counter + terminal_cnt_next <= terminal_cnt + 1; + end if; + stage_next <= RETURN_USER; + return_code_latch_next <= ROS_RET_OK; + -- Trigger Status Update + trigger_status_next <= '1'; + end if; + when others => + null; + end case; + when GET_R_DATA => + assert (rrq_occupied_head /= RRQ_MEMORY_MAX_ADDRESS) severity FAILURE; + + case (cnt) is + -- GET Data + when 0 => + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= GET; + mem_r.field_flags <= GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG; + mem_r.addr <= resize(unsigned(goal_handle_latch),GOAL_MEMORY_ADDR_WIDTH); + + cnt_next <= cnt + 1; + end if; + -- READ Data + when 1 => + -- Wait for Memory + if (mem_done = '1') then + assert check_mask(mem_data.field_flags, GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG) severity FAILURE; + assert (is_terminal(mem_data.state)) severity FAILURE; + + mem_data_r_next <= mem_data; + + if (mem_data.state /= GoalStatus_package.STATUS_SUCCEEDED) then + -- NOTE: We are setting result index to MAX_GOALS to effectively "zero" the result. + -- This prevents sending the result of previous succeeded goals, if the current + -- goal is not succeeded. + mem_data_r_next.res_ind <= std_logic_vector(to_unsigned(MAX_GOALS, WORD_WIDTH)); + end if; + + rrq_addr_base_next <= rrq_occupied_head; + stage_next <= CHECK_REQUESTS; + cnt_next <= 0; + end if; + when others => + null; + end case; + when CHECK_REQUESTS => + case (cnt) is + -- GET Goal Handle + when 0 => + -- NOTE: We check the base here, because a REMOVE_REQUEST operation may reset the rrq_addr_base + if (rrq_addr_base = RRQ_MEMORY_MAX_ADDRESS) then + -- Reset + trigger_result_next <= '0'; + + -- DONE + stage_next <= IDLE; + else + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_GOAL_HANDLE_OFFSET; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + end if; + -- GET Next Addr + when 1 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- READ Goal Handle + when 2 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + + -- Found Result Request of Updated Goal + if (resize(unsigned(rrq_read_data),GOAL_MEMORY_ADDR_WIDTH) = mem_data_r.addr) then + stage_next <= GET_REQUEST_ID; + cnt_next <= 0; + rrq_abort_read <= '1'; + else + cnt_next <= cnt + 1; + end if; + end if; + -- READ Next Addr + when 3 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + rrq_addr_base_next <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH); + + -- No More Stored Requests + if (resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH) = RRQ_MEMORY_MAX_ADDRESS) then + -- Reset + trigger_result_next <= '0'; + + stage_next <= IDLE; + else + -- Next Request + cnt_next <= 0; + end if; + end if; + when others => + null; + end case; + when GET_REQUEST_ID => + case (cnt) is + -- GET Request ID 1/6 + when 0 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- GET Request ID 2/6 + when 1 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 1; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- GET Request ID 3/6 + when 2 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 2; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- GET Request ID 4/6 + when 3 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 3; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- GET Request ID 5/6 + when 4 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 4; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- GET Request ID 6/6 + when 5 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 5; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- READ Request ID 1/6 + when 6 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + request_id_latch_next.writer_guid(0) <= rrq_read_data; + + cnt_next <= cnt + 1; + end if; + -- READ Request ID 2/6 + when 7 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + request_id_latch_next.writer_guid(1) <= rrq_read_data; + + cnt_next <= cnt + 1; + end if; + -- READ Request ID 3/6 + when 8 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + request_id_latch_next.writer_guid(2) <= rrq_read_data; + + cnt_next <= cnt + 1; + end if; + -- READ Request ID 4/6 + when 9 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + request_id_latch_next.writer_guid(3) <= rrq_read_data; + + cnt_next <= cnt + 1; + end if; + -- READ Request ID 5/6 + when 10 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + request_id_latch_next.sequence_number(0) <= unsigned(rrq_read_data); + + cnt_next <= cnt + 1; + end if; + -- READ Request ID 6/6 + when 11 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + request_id_latch_next.sequence_number(1) <= unsigned(rrq_read_data); + + stage_next <= REMOVE_REQUEST; + cnt_next <= 0; + end if; + when others => + null; + end case; + when REMOVE_REQUEST => + -- NOTE: After this stage the rrq_addr_base is set to the Next request + case (cnt) is + -- GET Next Addr + when 0 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- GET Prev Addr + when 1 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_PREV_ADDR_OFFSET; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- SET Next Addr + when 2 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET; + rrq_write_data <= std_logic_vector(resize(rrq_empty_head,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + -- Set New Empty Head + rrq_empty_head_next <= rrq_addr_base; + + cnt_next <= cnt + 1; + end if; + -- READ Next Addr + when 3 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + rrq_addr_latch_next <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH); + cnt_next <= cnt + 1; + end if; + -- READ Previous Addr + when 4 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + if (rrq_addr_latch = RRQ_MEMORY_MAX_ADDRESS) then + if (resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH) = RRQ_MEMORY_MAX_ADDRESS) then + -- Reset Occupied Head + rrq_occupied_head_next <= RRQ_MEMORY_MAX_ADDRESS; + + rrq_addr_base_next <= RRQ_MEMORY_MAX_ADDRESS; + stage_next <= SEND_R_RESPONSE; + else + rrq_addr_base_next <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH); + cnt_next <= cnt + 2; -- Skip Next Slot SET + end if; + else + rrq_addr_base_next <= rrq_addr_latch; + rrq_addr_latch_next <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH); + cnt_next <= cnt + 1; + end if; + end if; + -- SET Previous Addr (Next Slot) + when 5 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_PREV_ADDR_OFFSET; + rrq_write_data <= std_logic_vector(resize(rrq_addr_latch,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + if (rrq_addr_latch = RRQ_MEMORY_MAX_ADDRESS) then + -- Set New Occupied Head + rrq_occupied_head_next <= rrq_addr_base; + + stage_next <= SEND_R_RESPONSE; + else + rrq_addr_base_next <= rrq_addr_latch; + rrq_addr_latch_next <= rrq_addr_base; + cnt_next <= cnt + 1; + end if; + end if; + -- SET Next Addr (Previous Slot) + when 6 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET; + rrq_write_data <= std_logic_vector(resize(rrq_addr_latch,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + + rrq_addr_base_next <= rrq_addr_latch; + stage_next <= SEND_R_RESPONSE; + end if; + when others => + null; + end case; + when WAIT_FOR_U_MEM => + -- Wait for Memory + if (mem_done = '1') then + assert check_mask(mem_data.field_flags, GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG) severity FAILURE; + + mem_data_r_next <= mem_data; + + if (mem_data.addr /= GOAL_MEMORY_MAX_ADDRESS and mem_data.state = GoalStatus_package.STATUS_UNKNOWN) then + return_code_latch_next <= ROS_RET_ACTION_GOAL_HANDLE_INVALID; + end if; + + stage_next <= RETURN_USER; + end if; + when RETURN_USER => + -- End of Memory + if (mem_data_r.addr = GOAL_MEMORY_MAX_ADDRESS) then + goal_handle_out <= GOAL_HANDLE_UNKNOWN; + else + goal_handle_out <= std_logic_vector(resize(mem_data_r.addr, GOAL_HANDLE_WIDTH)); + end if; + + done <= '1'; + return_code <= return_code_latch; + + if (trigger_result = '1') then + -- No stored Result Requests + if (rrq_occupied_head = RRQ_MEMORY_MAX_ADDRESS) then + -- Reset + trigger_result_next <= '0'; + + stage_next <= IDLE; + else + stage_next <= GET_R_DATA; + cnt_next <= 0; + end if; + else + stage_next <= IDLE; + end if; + when PUBLISH_FEEDBACK => + case (cnt) is + -- Wait for Data + when 0 => + if (mem_done = '1') then + assert check_mask(mem_data.field_flags, GMF_GOAL_ID_FLAG) severity FAILURE; + + mem_data_r_next <= mem_data; + cnt_next <= cnt + 1; + end if; + -- Start Publish + when 1 => + start_fb <= '1'; + opcode_fb <= PUBLISH; + + if (ack_fb = '1') then + cnt_next <= cnt + 1; + end if; + -- Wait for Publish + when 2 => + -- Passthrough + done <= done_fb; + return_code <= return_code_fb; + + if (done_fb = '1') then + -- DONE + stage_next <= IDLE; + end if; + when others => + null; + end case; + when WAIT_FOR_G_DATA => + if (done_g = '1') then + case (return_code_g) is + when ROS_RET_OK => + if (taken_g = '1') then + stage_next <= WAIT_FOR_G_USER; + else + stage_next <= IDLE; + end if; + when others => + -- TODO: Propagate Error? + stage_next <= IDLE; + end case; + end if; + when WAIT_FOR_G_USER => + new_goal_request <= '1'; + + if (new_goal_response = '1') then + g_stamp_next <= time; + if (new_goal_accepted = '1') then + stage_next <= ADD_GOAL; + g_accept_next <= '1'; + else + stage_next <= SEND_G_RESPONSE; + g_accept_next <= '0'; + end if; + end if; + when ADD_GOAL => + assert (mem_empty_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= INSERT; + + stage_next <= SEND_G_RESPONSE; + -- Trigger Status Update + trigger_status_next <= '1'; + end if; + when SEND_G_RESPONSE => + start_g <= '1'; + opcode_g <= SEND_RESPONSE; + if (ack_g = '1') then + stage_next <= WAIT_FOR_G_RET; + end if; + when WAIT_FOR_G_RET => + if (done_g = '1') then + -- TODO: Propagate Error? + stage_next <= IDLE; + end if; + when WAIT_FOR_R_DATA => + if (done_r = '1') then + case (return_code_r) is + when ROS_RET_OK => + if (taken_r = '1') then + -- Latch Request ID + request_id_latch_next <= service_info_r.request_id; + + -- No Goals Available + if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then + mem_data_r_next.res_ind <= std_logic_vector(to_unsigned(MAX_GOALS, WORD_WIDTH)); + mem_data_r_next.state <= GoalStatus_package.STATUS_UNKNOWN; + mem_data_r_next.field_flags <= GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG; + stage_next <= SEND_R_RESPONSE; + else + stage_next <= RESULT_CHECK; + cnt_next <= 0; + end if; + else + stage_next <= IDLE; + end if; + when others => + -- TODO: Propagate Error? + stage_next <= IDLE; + end case; + end if; + when RESULT_CHECK => + case (cnt) is + -- GET FIRST + when 0 => + assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= GET; + mem_r.field_flags <= GMF_GOAL_ID_FLAG; + mem_r.addr <= mem_occupied_head; + + cnt_next <= cnt + 2; + end if; + -- GET NEXT + when 1 => + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= GET_NEXT; + mem_r.field_flags <= GMF_GOAL_ID_FLAG; + mem_r.addr <= mem_data_r.addr; + + cnt_next <= cnt + 1; + end if; + -- CHECK + when 2 => + -- Wait for Memory + if (mem_done = '1') then + assert check_mask(mem_data.field_flags, GMF_GOAL_ID_FLAG) severity FAILURE; + + -- No more Goals + if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then + -- Goal not found + mem_data_r_next.res_ind <= std_logic_vector(to_unsigned(MAX_GOALS, WORD_WIDTH)); + mem_data_r_next.state <= GoalStatus_package.STATUS_UNKNOWN; + mem_data_r_next.field_flags <= GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG; + stage_next <= SEND_R_RESPONSE; + else + mem_data_r_next <= mem_data; + cnt_next <= cnt + 1; + end if; + end if; + -- CHECK GOAL_ID 1/4 + when 3 => + if (mem_data_r.goal_id(0) = to_GUID(goal_id_r)(0)) then + cnt_next <= cnt + 1; + else + cnt_next <= 1; -- GET_NEXT + end if; + -- CHECK GOAL_ID 2/4 + when 4 => + if (mem_data_r.goal_id(1) = to_GUID(goal_id_r)(1)) then + cnt_next <= cnt + 1; + else + cnt_next <= 1; -- GET_NEXT + end if; + -- CHECK GOAL_ID 3/4 + when 5 => + if (mem_data_r.goal_id(2) = to_GUID(goal_id_r)(2)) then + cnt_next <= cnt + 1; + else + cnt_next <= 1; -- GET_NEXT + end if; + -- CHECK GOAL_ID 4/4 + when 6 => + if (mem_data_r.goal_id(3) = to_GUID(goal_id_r)(3)) then + -- Found Goal + cnt_next <= cnt + 1; + else + cnt_next <= 1; -- GET_NEXT + end if; + -- GET STATE + when 7 => + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= GET; + mem_r.field_flags <= GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG; + mem_r.addr <= mem_data_r.addr; + + cnt_next <= cnt + 1; + end if; + -- Check State + when 8 => + -- Wait for Memory + if (mem_done = '1') then + assert check_mask(mem_data.field_flags, GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG) severity FAILURE; + + mem_data_r_next <= mem_data; + + if (is_terminal(mem_data.state)) then + if (mem_data.state /= GoalStatus_package.STATUS_SUCCEEDED) then + -- NOTE: We are setting result index to MAX_GOALS to effectively "zero" the result. + -- This prevents sending the result of previous succeeded goals, if the current + -- goal is not succeeded. + mem_data_r_next.res_ind <= std_logic_vector(to_unsigned(MAX_GOALS, WORD_WIDTH)); + end if; + stage_next <= SEND_R_RESPONSE; + else + stage_next <= STORE_RRQ; + cnt_next <= 0; + end if; + end if; + when others => + null; + end case; + when STORE_RRQ => + assert (rrq_empty_head /= RRQ_MEMORY_MAX_ADDRESS) severity FAILURE; + + case (cnt) is + -- GET Next Addr + when 0 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_empty_head + RMF_NEXT_ADDR_OFFSET; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- SET Goal Handle + when 1 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_empty_head + RMF_GOAL_HANDLE_OFFSET; + rrq_write_data <= std_logic_vector(resize(mem_data_r.addr,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- SET Request ID 1/6 + when 2 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET; + rrq_write_data <= request_id_latch.writer_guid(0); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- SET Request ID 2/6 + when 3 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 1; + rrq_write_data <= request_id_latch.writer_guid(1); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- SET Request ID 3/6 + when 4 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 2; + rrq_write_data <= request_id_latch.writer_guid(2); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- SET Request ID 4/6 + when 5 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 3; + rrq_write_data <= request_id_latch.writer_guid(3); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- SET Request ID 5/6 + when 6 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 4; + rrq_write_data <= std_logic_vector(request_id_latch.sequence_number(0)); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- SET Request ID 6/6 + when 7 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 5; + rrq_write_data <= std_logic_vector(request_id_latch.sequence_number(1)); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + if (rrq_occupied_head = RRQ_MEMORY_MAX_ADDRESS) then + cnt_next <= cnt + 2; + else + cnt_next <= cnt + 1; + end if; + end if; + -- SET Prev Addr (Old Occupied Head) + when 8 => + assert (rrq_occupied_head /= RRQ_MEMORY_MAX_ADDRESS) severity FAILURE; + + rrq_valid_in <= '1'; + rrq_addr <= rrq_occupied_head + RMF_PREV_ADDR_OFFSET; + rrq_write_data <= std_logic_vector(resize(rrq_empty_head,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- SET Next Addr + when 9 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_empty_head + RMF_NEXT_ADDR_OFFSET; + rrq_write_data <= std_logic_vector(resize(rrq_occupied_head,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + -- Set New Occupied Head + rrq_occupied_head_next <= rrq_empty_head; + cnt_next <= cnt + 1; + end if; + -- SET Prev Addr + when 10 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_empty_head + RMF_PREV_ADDR_OFFSET; + rrq_write_data <= std_logic_vector(resize(RRQ_MEMORY_MAX_ADDRESS,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- READ Next Addr + when 11 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + -- Set new Empty Head + rrq_empty_head_next <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH); + -- DONE + stage_next <= IDLE; + end if; + when others => + null; + end case; + when SEND_R_RESPONSE => + assert check_mask(mem_data_r.field_flags, GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG) severity FAILURE; + result_sel <= '1'; + + if (result_sel_ack = '1') then + start_r <= '1'; + opcode_r <= SEND_RESPONSE; + + if (ack_r = '1') then + stage_next <= WAIT_FOR_R_RET; + end if; + end if; + when WAIT_FOR_R_RET => + assert check_mask(mem_data_r.field_flags, GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG) severity FAILURE; + result_sel <= '1'; + + if (done_r = '1') then + -- TODO: Propagate Error? + stage_next <= IDLE; + + if (trigger_result = '1') then + stage_next <= CHECK_REQUESTS; + cnt_next <= 0; + end if; + end if; + when WAIT_FOR_C_DATA => + if (done_c = '1') then + case (return_code_c) is + when ROS_RET_OK => + if (taken_c = '1') then + -- No Goals Available + if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then + cancel_ret_code_next <= CancelGoal_package.RR_ERROR_REJECTED; + stage_next <= SEND_C_RESPONSE; + else + if (to_UUID(goal_info_goal_id_c) = UUID_UNKNOWN) then + if (to_ROS_TIME(goal_info_stamp_c) = ROS_TIME_ZERO) then + search_type_c_next <= SEARCH_NONE; + else + search_type_c_next <= SEARCH_STAMP; + end if; + else + search_type_c_next <= SEARCH_GOAL_ID; + end if; + stage_next <= GET_C_DATA; + cnt_next <= 0; -- GET_FIRST + end if; + else + stage_next <= IDLE; + end if; + when others => + -- TODO: Propagate Error? + stage_next <= IDLE; + end case; + end if; + when GET_C_DATA => + case (cnt) is + -- GET FIRST + when 0 => + -- Memory Operation Guard + if (mem_done = '1') then + assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + mem_start <= '1'; + mem_opcode <= GET; + mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG; + mem_r.addr <= mem_occupied_head; + + stage_next <= CANCEL_CHECK; + cnt_next <= 0; + end if; + -- GET NEXT + when 1 => + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= GET_NEXT; + mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG; + mem_r.addr <= mem_data_r.addr; + + stage_next <= CANCEL_CHECK; + cnt_next <= 0; + end if; + when others => + null; + end case; + when CANCEL_CHECK => + case (cnt) is + -- WAIT FOR MEMORY + when 0 => + -- Wait for Memory + if (mem_done = '1') then + -- End of Memory + if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then + case (search_type_c) is + when SEARCH_GOAL_ID => + cancel_ret_code_next <= CancelGoal_package.RR_ERROR_UNKNOWN_GOAL_ID; + stage_next <= SEND_C_RESPONSE; + when others => + -- No Goals Canceled + if (goals_canceling_cnt = 0) then + cancel_ret_code_next <= CancelGoal_package.RR_ERROR_REJECTED; + stage_next <= SEND_C_RESPONSE; + else + cancel_ret_code_next <= CancelGoal_package.RR_ERROR_NONE; + stage_next <= SEND_C_RESPONSE; + end if; + end case; + else + assert check_mask(mem_data.field_flags, GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG) severity FAILURE; + + mem_data_r_next <= mem_data; + + case (search_type_c) is + when SEARCH_GOAL_ID => + cnt_next <= 1; -- CHECK GOAL_ID + when SEARCH_STAMP => + cnt_next <= 5; -- CHECK STAMP + when SEARCH_NONE => + cnt_next <= 6; -- CHECK STATE + end case; + end if; + end if; + -- CHECK GOAL_ID 1/4 + when 1 => + if (mem_data_r.goal_id(0) = to_guid(goal_info_goal_id_c)(0)) then + cnt_next <= cnt + 1; + else + stage_next <= GET_C_DATA; + cnt_next <= 1; -- GET_NEXT + end if; + -- CHECK GOAL_ID 2/4 + when 2 => + if (mem_data_r.goal_id(1) = to_guid(goal_info_goal_id_c)(1)) then + cnt_next <= cnt + 1; + else + stage_next <= GET_C_DATA; + cnt_next <= 1; -- GET_NEXT + end if; + -- CHECK GOAL_ID 3/4 + when 3 => + if (mem_data_r.goal_id(2) = to_guid(goal_info_goal_id_c)(2)) then + cnt_next <= cnt + 1; + else + stage_next <= GET_C_DATA; + cnt_next <= 1; -- GET_NEXT + end if; + -- CHECK GOAL_ID 4/4 + when 4 => + if (mem_data_r.goal_id(3) = to_guid(goal_info_goal_id_c)(3)) then + cnt_next <= cnt + 2; + else + stage_next <= GET_C_DATA; + cnt_next <= 1; -- GET_NEXT + end if; + -- CHECK STAMP + when 5 => + if (mem_data_r.stamp <= to_ROS_TIME(goal_info_stamp_c)) then + cnt_next <= cnt + 1; + else + stage_next <= GET_C_DATA; + cnt_next <= 1; -- GET_NEXT + end if; + -- CHECK STATE + when 6 => + if (is_terminal(mem_data_r.state)) then + if (search_type_c = SEARCH_GOAL_ID) then + cancel_ret_code_next <= CancelGoal_package.RR_ERROR_GOAL_TERMINATED; + stage_next <= SEND_C_RESPONSE; + else + stage_next <= GET_C_DATA; + cnt_next <= 1; -- GET_NEXT + end if; + elsif (mem_data_r.state = GoalStatus_package.STATUS_CANCELING) then + -- Skip State Change + stage_next <= ADD_CANCEL; + else + stage_next <= WAIT_FOR_C_USER; + end if; + when others => + null; + end case; + when WAIT_FOR_C_USER => + cancel_request <= '1'; + cancel_request_handle <= std_logic_vector(resize(mem_data_r.addr,GOAL_HANDLE_WIDTH)); + if (cancel_response = '1') then + if (cancel_accepted = '1') then + stage_next <= CANCEL_GOAL; + else + case (search_type_c) is + when SEARCH_GOAL_ID => + if (to_ROS_TIME(goal_info_stamp_c) = ROS_TIME_ZERO) then + -- No Goals Canceled + cancel_ret_code_next <= CancelGoal_package.RR_ERROR_REJECTED; + stage_next <= SEND_C_RESPONSE; + else + -- Search for Timestamps + search_type_c_next <= SEARCH_STAMP; + stage_next <= GET_C_DATA; + cnt_next <= 0; -- GET_FIRST + end if; + when SEARCH_STAMP => + -- NOTE: Since the goals are added chronologically, we don't have to check the timetsamps from now on + search_type_c_next <= SEARCH_NONE; + stage_next <= GET_C_DATA; + cnt_next <= 1; -- GET_NEXT + when SEARCH_NONE => + -- Continue + stage_next <= GET_C_DATA; + cnt_next <= 1; -- GET_NEXT + end case; + end if; + end if; + when CANCEL_GOAL => + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= UPDATE; + mem_r.addr <= mem_data_r.addr; + mem_r.field_flags <= GMF_STATE_FLAG; + mem_r.state <= GoalStatus_package.STATUS_CANCELING; + + stage_next <= ADD_CANCEL; + -- Trigger Status Update + trigger_status_next <= '1'; + end if; + when ADD_CANCEL => + goals_canceling_wen_c <= '1'; + + if (goals_canceling_ready_c = '1') then + -- Reached maximum number of cancel goals + if (goals_canceling_cnt = CancelGoal_package.RR_GOALS_CANCELING_MAX_DEPTH) then + -- DONE + cancel_ret_code_next <= CancelGoal_package.RR_ERROR_NONE; + stage_next <= SEND_C_RESPONSE; + else + goals_canceling_cnt_next <= goals_canceling_cnt + 1; + + case (search_type_c) is + when SEARCH_GOAL_ID => + if (to_ROS_TIME(goal_info_stamp_c) = ROS_TIME_ZERO) then + -- DONE + cancel_ret_code_next <= CancelGoal_package.RR_ERROR_NONE; + stage_next <= SEND_C_RESPONSE; + else + -- Search for Timetamps + search_type_c_next <= SEARCH_STAMP; + stage_next <= GET_C_DATA; + cnt_next <= 0; -- GET_FIRST + end if; + when SEARCH_STAMP => + -- NOTE: Since the goals are added chronologically, we don't have to check the timetsamps from now on + search_type_c_next <= SEARCH_NONE; + stage_next <= GET_C_DATA; + cnt_next <= 1; -- GET_NEXT + when SEARCH_NONE => + -- Continue + stage_next <= GET_C_DATA; + cnt_next <= 1; -- GET_NEXT + end case; + end if; + end if; + when SEND_C_RESPONSE => + start_c <= '1'; + opcode_c <= SEND_RESPONSE; + + if (ack_c = '1') then + stage_next <= WAIT_FOR_C_RET; + end if; + when WAIT_FOR_C_RET => + if (done_c = '1') then + -- TODO: Propagate Error? + stage_next <= IDLE; + end if; + when CHECK_TIMEOUT => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then + case (cnt) is + -- GET + when 0 => + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= GET; + mem_r.field_flags <= GMF_DEADLINE_FLAG; + mem_r.addr <= mem_data_r.addr; + + cnt_next <= cnt + 2; + end if; + -- GET NEXT + when 1 => + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= GET_NEXT; + mem_r.field_flags <= GMF_DEADLINE_FLAG; + mem_r.addr <= mem_data_r.addr; + + cnt_next <= cnt + 1; + end if; + -- CHECK + when 2 => + -- Wait for Memory + if (mem_done = '1') then + assert check_mask(mem_data.field_flags, GMF_DEADLINE_FLAG) severity FAILURE; + + mem_data_r_next <= mem_data; + + -- No more Goals + if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then + -- DONE + stage_next <= IDLE; + else + -- Timeout + if (mem_data.deadline <= time) then + cnt_next <= cnt + 1; + else + -- Update check Time + if (mem_data.deadline < check_time) then + check_time_next <= mem_data.deadline; + end if; + cnt_next <= 1; -- GET_NEXT + end if; + end if; + end if; + -- REMOVE + when 3 => + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= REMOVE; + mem_r.addr <= mem_data_r.addr; + + cnt_next <= cnt + 1; + -- Trigger Status Update + trigger_status_next <= '1'; + -- Update Terminal Goal Counter + terminal_cnt_next <= terminal_cnt - 1; + end if; + -- Wait for Remove + when 4 => + -- Wait for Memory + if (mem_done = '1') then + mem_data_r_next <= mem_data; + + -- NOTE: After Removal, mem_data.addr is set to the next goal, so we need to call GET and not GET_NEXT + -- No more Goals + if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then + -- DONE + stage_next <= IDLE; + else + cnt_next <= 0; -- GET + end if; + end if; + when others => + null; + end case; + end if; + when REMOVE_OLDEST => + -- Synthesis Guard + if (TIMEOUT_DURATION = ROS_DURATION_INFINITE) then + assert (terminal_cnt > 0) severity FAILURE; + + case (cnt) is + -- GET + when 0 => + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= GET; + mem_r.field_flags <= GMF_STATE_FLAG; + mem_r.addr <= mem_data_r.addr; + + cnt_next <= cnt + 2; + end if; + -- GET NEXT + when 1 => + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= GET_PREV; + mem_r.field_flags <= GMF_STATE_FLAG; + mem_r.addr <= mem_data_r.addr; + + cnt_next <= cnt + 1; + end if; + -- CHECK + when 2 => + -- Wait for Memory + if (mem_done = '1') then + assert check_mask(mem_data.field_flags, GMF_STATE_FLAG) severity FAILURE; + + mem_data_r_next <= mem_data; + + -- No more Goals + if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then + assert FALSE report "REMOVE_OLDEST did not find goal to remove" severity FAILURE; + + stage_next <= IDLE; + else + if (is_terminal(mem_data.state)) then + cnt_next <= cnt + 1; + else + cnt_next <= 1; -- GET_PREV + end if; + end if; + end if; + -- REMOVE + when 3 => + -- Memory Operation Guard + if (mem_done = '1') then + mem_start <= '1'; + mem_opcode <= REMOVE; + mem_r.addr <= mem_data_r.addr; + + -- Trigger Status Update + trigger_status_next <= '1'; + -- Update Terminal Goal Counter + terminal_cnt_next <= terminal_cnt - 1; + + -- DONE + stage_next <= IDLE; + end if; + when others => + null; + end case; + end if; + when RESET_MEMORY => + case (cnt) is + -- SET Next Pointer + when 0 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET; + if (rrq_addr_base = MAX_RRQ_ADDRESS) then + rrq_write_data <= std_logic_vector(resize(RRQ_MEMORY_MAX_ADDRESS,WORD_WIDTH)); + else + rrq_write_data <= std_logic_vector(resize(rrq_addr_base + RRQ_FRAME_SIZE,WORD_WIDTH)); + end if; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- SET Previous Pointer + when 1 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_PREV_ADDR_OFFSET; + rrq_write_data <= std_logic_vector(resize(rrq_addr_latch,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + if (rrq_addr_base = MAX_RRQ_ADDRESS) then + -- Initialize Empty and Occupied Heads + rrq_empty_head_next <= FIRST_RRQ_ADDRESS; + rrq_occupied_head_next <= RRQ_MEMORY_MAX_ADDRESS; + + -- DONE + stage_next <= IDLE; + else + rrq_addr_latch_next <= rrq_addr_base; + rrq_addr_base_next <= rrq_addr_base + RRQ_FRAME_SIZE; + cnt_next <= 0; + end if; + end if; + when others => + null; + end case; + end case; + end process; + + + -- *Goal Memory State Machine* + -- STATE DESCRIPTION + -- IDLE Idle State. Done Signal is pulled high and Memory FSM accepts new memory operations + -- INSERT See Memory OPCODE Description + -- GET See Memory OPCODE Description + -- GET_NEXT See Memory OPCODE Description + -- GET_PREV See Memory OPCODE Description + -- UPDATE See Memory OPCODE Description + -- GET_RESULT_INDEX Set the empty_head_res_ind before REMOVE + -- REMOVE See Memory OPCODE Description + -- RESET_MEMORY Reset goal memory to empty state + mem_ctrl_prc : process(all) + begin + -- DEFAULT + mem_stage_next <= mem_stage; + mem_cnt_next <= mem_cnt; + mem_occupied_head_next <= mem_occupied_head; + mem_occupied_tail_next <= mem_occupied_tail; + mem_empty_head_next <= mem_empty_head; + mem_empty_tail_next <= mem_empty_tail; + empty_head_res_ind_next <= empty_head_res_ind; + mem_latch_data_next <= mem_latch_data; + mem_data_next <= mem_data; + mem_addr_base_next <= mem_addr_base; + mem_addr_latch_next <= mem_addr_latch; + -- DEFAULT unregistered + mem_done <= '0'; + mem_abort_read <= '0'; + mem_ready_out <= '0'; + mem_valid_in <= '0'; + mem_read <= '0'; + mem_done <= '0'; + mem_addr <= (others => '0'); + mem_write_data <= (others => '0'); + + + case (mem_stage) is + -- NOTE: The REMOVE opcode sets mem_data.addr to the next slot (or GOAL_MEMORY_MAX_ADDRESS if no next goals) + when IDLE => + mem_done <= '1'; + if (mem_start = '1') then + + case (mem_opcode) is + when INSERT => + assert (mem_empty_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + -- Latch DATA + mem_latch_data_next.goal_id <= to_guid(goal_id_g); + mem_latch_data_next.stamp <= g_stamp; + + mem_addr_base_next <= mem_empty_head; + mem_stage_next <= INSERT; + mem_cnt_next <= 0; + when GET => + assert (mem_r.addr /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + -- Latch DATA + mem_latch_data_next <= mem_r; + + if (mem_r.addr = mem_addr_base) then + mem_data_next.field_flags <= mem_data.field_flags or mem_r.field_flags; + else + mem_data_next <= ZERO_GOAL_DATA; + mem_data_next.field_flags <= mem_r.field_flags; + end if; + mem_data_next.addr <= mem_r.addr; + + mem_addr_base_next <= mem_r.addr; + mem_stage_next <= GET; + + if check_mask(mem_r.field_flags,GMF_STATE_FLAG) then + mem_cnt_next <= 0; + elsif check_mask(mem_r.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 1; + elsif check_mask(mem_r.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 5; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_r.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_r.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + -- DONE + mem_stage_next <= IDLE; + end if; + when GET_NEXT => + assert (mem_r.addr /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + -- Latch DATA + mem_latch_data_next <= mem_r; + + mem_data_next <= ZERO_GOAL_DATA; + mem_data_next.field_flags <= mem_r.field_flags; + + mem_addr_base_next <= mem_r.addr; + mem_stage_next <= GET_NEXT; + mem_cnt_next <= 0; + when GET_PREV => + assert (mem_r.addr /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + -- Latch DATA + mem_latch_data_next <= mem_r; + + mem_data_next <= ZERO_GOAL_DATA; + mem_data_next.field_flags <= mem_r.field_flags; + + mem_addr_base_next <= mem_r.addr; + mem_stage_next <= GET_PREV; + mem_cnt_next <= 0; + when UPDATE => + assert (mem_r.addr /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + -- Latch DATA + mem_latch_data_next <= mem_r; + + if (mem_r.addr = mem_addr_base) then + mem_data_next.field_flags <= mem_data.field_flags or mem_r.field_flags; + else + mem_data_next <= ZERO_GOAL_DATA; + mem_data_next.field_flags <= mem_r.field_flags; + end if; + mem_data_next.addr <= mem_r.addr; + + mem_addr_base_next <= mem_r.addr; + mem_stage_next <= UPDATE; + + if check_mask(mem_r.field_flags,GMF_STATE_FLAG) then + mem_cnt_next <= 0; + elsif check_mask(mem_r.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 1; + elsif check_mask(mem_r.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 5; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_r.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_r.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + -- DONE + mem_stage_next <= IDLE; + end if; + when REMOVE => + assert (mem_r.addr /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + mem_addr_base_next <= mem_r.addr; + + if (mem_empty_head = GOAL_MEMORY_MAX_ADDRESS) then + mem_stage_next <= GET_RESULT_INDEX; + mem_cnt_next <= 0; + else + mem_stage_next <= REMOVE; + mem_cnt_next <= 0; + end if; + when others => + null; + end case; + end if; + when INSERT => + case (mem_cnt) is + -- GET Next Addr + when 0 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- READ Next Addr + when 1 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + -- SET New Empty Head + mem_empty_head_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); + + if (resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH) = GOAL_MEMORY_MAX_ADDRESS) then + assert (mem_empty_head = mem_empty_tail) severity FAILURE; + + -- SET New Empty Tail + mem_empty_tail_next <= GOAL_MEMORY_MAX_ADDRESS; + mem_cnt_next <= mem_cnt + 3; -- Skip Result index + else + mem_cnt_next <= mem_cnt + 1; + end if; + end if; + -- GET Result Index (New Empty Head) + when 2 => + assert (mem_empty_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + mem_valid_in <= '1'; + mem_addr <= mem_empty_head + GMF_RESULT_INDEX_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- READ Result Index (New Empty Head) + when 3 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + empty_head_res_ind_next <= unsigned(mem_read_data); + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Goal State + when 4 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STATE_OFFSET; + mem_write_data(CDR_INT8_WIDTH-1 downto 0) <= GoalStatus_package.STATUS_ACCEPTED; + mem_data_next.state <= GoalStatus_package.STATUS_ACCEPTED; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Goal ID 1/4 + when 5 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET; + mem_write_data <= mem_latch_data.goal_id(0); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Goal ID 2/4 + when 6 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 1; + mem_write_data <= mem_latch_data.goal_id(1); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Goal ID 3/4 + when 7 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 2; + mem_write_data <= mem_latch_data.goal_id(2); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Goal ID 4/4 + when 8 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 3; + mem_write_data <= mem_latch_data.goal_id(3); + mem_data_next.goal_id <= mem_latch_data.goal_id; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Stamp 1/2 + when 9 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STAMP_OFFSET; + mem_write_data <= mem_latch_data.stamp.sec; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Stamp 2/2 + when 10 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STAMP_OFFSET + 1; + mem_write_data <= mem_latch_data.stamp.nanosec; + mem_data_next.stamp <= mem_latch_data.stamp; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then + mem_cnt_next <= mem_cnt + 1; + else + mem_cnt_next <= mem_cnt + 3; + end if; + end if; + -- SET Deadline 1/2 + when 11 => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET; + mem_write_data <= ROS_TIME_INFINITE.sec; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + end if; + -- SET Deadline 2/2 + when 12 => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET + 1; + mem_write_data <= ROS_TIME_INFINITE.nanosec; + mem_data_next.deadline <= ROS_TIME_INFINITE; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + end if; + -- SET Prev Addr + when 13 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_PREV_ADDR_OFFSET; + mem_write_data <= std_logic_vector(resize(GOAL_MEMORY_MAX_ADDRESS,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then + mem_cnt_next <= mem_cnt + 2; -- Skip next Step + else + mem_cnt_next <= mem_cnt + 1; + end if; + end if; + -- SET Prev Addr (Current Occupied Head) + when 14 => + assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + mem_valid_in <= '1'; + mem_addr <= mem_occupied_head + GMF_PREV_ADDR_OFFSET; + mem_write_data <= std_logic_vector(resize(mem_addr_base,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Next Addr + when 15 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET; + mem_write_data <= std_logic_vector(resize(mem_occupied_head,WORD_WIDTH)); + mem_data_next.addr <= mem_addr_base; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + -- Update Occupied Head + mem_occupied_head_next <= mem_addr_base; + -- Initial Occupied Tail + if (mem_occupied_tail = GOAL_MEMORY_MAX_ADDRESS) then + mem_occupied_tail_next <= mem_addr_base; + end if; + + -- DONE + mem_stage_next <= IDLE; + end if; + when others => + null; + end case; + when GET => + case (mem_cnt) is + -- GET Goal State + when 0 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STATE_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 1; + elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 5; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + mem_cnt_next <= 10; + end if; + end if; + -- GET Goal ID 1/4 + when 1 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- GET Goal ID 2/4 + when 2 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 1; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- GET Goal ID 3/4 + when 3 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 2; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- GET Goal ID 4/4 + when 4 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 3; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 5; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then + mem_cnt_next <= 10; + else + mem_cnt_next <= 11; + end if; + end if; + end if; + -- GET Stamp 1/2 + when 5 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STAMP_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- GET Stamp 2/2 + when 6 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STAMP_OFFSET + 1; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then + mem_cnt_next <= 10; + elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 11; + else + mem_cnt_next <= 15; + end if; + end if; + end if; + -- GET Deadline 1/2 + when 7 => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + end if; + -- GET Deadline 2/2 + when 8 => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET + 1; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then + mem_cnt_next <= 10; + elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 11; + elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 15; + else + mem_cnt_next <= 17; + end if; + end if; + end if; + end if; + -- GET Result Index + when 9 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_RESULT_INDEX_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then + mem_cnt_next <= 10; + elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 11; + elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 15; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 17; + else + mem_cnt_next <= 19; + end if; + end if; + -- READ Goal State + when 10 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_data_next.state <= mem_read_data(CDR_INT8_WIDTH-1 downto 0); + + if check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 11; + elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 15; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 17; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 19; + else + mem_stage_next <= IDLE; + end if; + end if; + -- READ Goal ID 1/4 + when 11 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_data_next.goal_id(0) <= mem_read_data; + + mem_cnt_next <= mem_cnt + 1; + end if; + -- READ Goal ID 2/4 + when 12 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_data_next.goal_id(1) <= mem_read_data; + + mem_cnt_next <= mem_cnt + 1; + end if; + -- READ Goal ID 3/4 + when 13 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_data_next.goal_id(2) <= mem_read_data; + + mem_cnt_next <= mem_cnt + 1; + end if; + -- READ Goal ID 4/4 + when 14 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_data_next.goal_id(3) <= mem_read_data; + + if check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 15; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 17; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 19; + else + mem_stage_next <= IDLE; + end if; + end if; + -- READ Stamp 1/2 + when 15 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_data_next.stamp.sec <= mem_read_data; + + mem_cnt_next <= mem_cnt + 1; + end if; + -- READ Stamp 2/2 + when 16 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_data_next.stamp.nanosec <= mem_read_data; + + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 17; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 19; + else + mem_stage_next <= IDLE; + end if; + end if; + -- READ Deadline 1/2 + when 17 => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_data_next.deadline.sec <= mem_read_data; + + mem_cnt_next <= mem_cnt + 1; + end if; + end if; + -- READ Deadline 2/2 + when 18 => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_data_next.deadline.nanosec <= mem_read_data; + + if check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 19; + else + mem_stage_next <= IDLE; + end if; + end if; + end if; + -- READ Result index + when 19 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_data_next.res_ind <= mem_read_data; + + mem_stage_next <= IDLE; + end if; + when others => + null; + end case; + when GET_NEXT => + case (mem_cnt) is + -- GET Goal State + when 0 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STATE_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- GET Next Addr + when 1 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- READ Goal State + when 2 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + + if (mem_read_data(CDR_INT8_WIDTH-1 downto 0) = GoalStatus_package.STATUS_UNKNOWN) then + mem_stage_next <= IDLE; + mem_abort_read <= '1'; + else + mem_cnt_next <= mem_cnt + 1; + end if; + end if; + -- READ Next Addr + when 3 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + + mem_data_next.addr <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); + + -- No Next Goal + if (resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH) = GOAL_MEMORY_MAX_ADDRESS) then + -- DONE + mem_stage_next <= IDLE; + else + mem_addr_base_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); + mem_stage_next <= GET; + + if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then + mem_cnt_next <= 0; + elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 1; + elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 5; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + -- DONE + mem_stage_next <= IDLE; + end if; + end if; + end if; + when others => + null; + end case; + when GET_PREV => + case (mem_cnt) is + -- GET Goal State + when 0 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STATE_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- GET Previous Addr + when 1 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_PREV_ADDR_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- READ Goal State + when 2 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + + if (mem_read_data(CDR_INT8_WIDTH-1 downto 0) = GoalStatus_package.STATUS_UNKNOWN) then + mem_stage_next <= IDLE; + mem_abort_read <= '1'; + else + mem_cnt_next <= mem_cnt + 1; + end if; + end if; + -- READ Previous Addr + when 3 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + + mem_data_next.addr <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); + + -- No Previous Goal + if (resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH) = GOAL_MEMORY_MAX_ADDRESS) then + -- DONE + mem_stage_next <= IDLE; + else + mem_addr_base_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); + mem_stage_next <= GET; + + if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then + mem_cnt_next <= 0; + elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 1; + elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 5; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + -- DONE + mem_stage_next <= IDLE; + end if; + end if; + end if; + when others => + null; + end case; + when UPDATE => + case (mem_cnt) is + -- SET Goal State + when 0 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STATE_OFFSET; + mem_write_data <= (others => '0'); + mem_write_data(CDR_INT8_WIDTH-1 downto 0) <= mem_latch_data.state; + mem_data_next.state <= mem_latch_data.state; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 1; + elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 5; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + mem_stage_next <= IDLE; + end if; + end if; + -- SET Goal ID 1/4 + when 1 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET; + mem_write_data <= mem_latch_data.goal_id(0); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Goal ID 2/4 + when 2 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 1; + mem_write_data <= mem_latch_data.goal_id(1); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Goal ID 3/4 + when 3 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 2; + mem_write_data <= mem_latch_data.goal_id(3); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Goal ID 4/4 + when 4 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 3; + mem_write_data <= mem_latch_data.goal_id(3); + mem_data_next.goal_id <= mem_latch_data.goal_id; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 5; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + mem_stage_next <= IDLE; + end if; + end if; + -- SET Stamp 1/2 + when 5 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STAMP_OFFSET; + mem_write_data <= mem_latch_data.stamp.sec; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Stamp 2/2 + when 6 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STAMP_OFFSET + 1; + mem_write_data <= mem_latch_data.stamp.nanosec; + mem_data_next.stamp <= mem_latch_data.stamp; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + mem_stage_next <= IDLE; + end if; + end if; + -- SET Deadline 1/2 + when 7 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET; + mem_write_data <= mem_latch_data.deadline.sec; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Deadline 2/2 + when 8 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET + 1; + mem_write_data <= mem_latch_data.deadline.nanosec; + mem_data_next.deadline <= mem_latch_data.deadline; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + mem_stage_next <= IDLE; + end if; + end if; + -- SET Result Index + when 9 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_RESULT_INDEX_OFFSET; + mem_write_data <= mem_latch_data.res_ind; + mem_data_next.res_ind <= mem_latch_data.res_ind; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + -- DONE + mem_stage_next <= IDLE; + end if; + when others => + null; + end case; + when GET_RESULT_INDEX => + assert (mem_empty_head = GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + case (mem_cnt) is + -- GET Next Addr + when 0 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_RESULT_INDEX_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- READ Next Addr + when 1 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + -- Update Empty Result Index + empty_head_res_ind_next <= unsigned(mem_read_data); + + mem_stage_next <= REMOVE; + mem_cnt_next <= 0; + end if; + when others => + null; + end case; + when REMOVE => + case (mem_cnt) is + -- GET Next Addr + when 0 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- GET Previous Addr + when 1 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_PREV_ADDR_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET State + when 2 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STATE_OFFSET; + mem_write_data(CDR_INT8_WIDTH-1 downto 0) <= GoalStatus_package.STATUS_UNKNOWN; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Next Addr + when 3 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET; + mem_write_data <= std_logic_vector(resize(GOAL_MEMORY_MAX_ADDRESS,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if (mem_empty_tail = GOAL_MEMORY_MAX_ADDRESS) then + -- Set New Empty Head/Tail + mem_empty_head_next <= mem_addr_base; + mem_empty_tail_next <= mem_addr_base; + mem_cnt_next <= mem_cnt + 2; -- Skip Next Step + else + mem_cnt_next <= mem_cnt + 1; + end if; + end if; + -- SET Next Addr (Current Tail) + when 4 => + assert (mem_empty_tail /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + mem_valid_in <= '1'; + mem_addr <= mem_empty_tail + GMF_NEXT_ADDR_OFFSET; + mem_write_data <= std_logic_vector(resize(mem_addr_base,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + -- Set New Empty TAIL + mem_empty_tail_next <= mem_addr_base; + mem_cnt_next <= mem_cnt + 1; + end if; + -- READ Next Addr + when 5 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_addr_latch_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); + -- Set mem_data.addr to previous slot + mem_data_next.addr <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); + + mem_cnt_next <= mem_cnt + 1; + end if; + -- READ Previous Addr + when 6 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + if (mem_addr_latch = GOAL_MEMORY_MAX_ADDRESS) then + -- Set New Occupied Tail + mem_occupied_tail_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); + + if (resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH) = GOAL_MEMORY_MAX_ADDRESS) then + -- Reset Occupied Head + mem_occupied_head_next <= GOAL_MEMORY_MAX_ADDRESS; + + mem_stage_next <= IDLE; + else + mem_addr_base_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); + mem_cnt_next <= mem_cnt + 2; + end if; + else + mem_addr_base_next <= mem_addr_latch; + mem_addr_latch_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); + mem_cnt_next <= mem_cnt + 1; + end if; + end if; + -- SET Previous Addr (Next Slot) + when 7 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_PREV_ADDR_OFFSET; + mem_write_data <= std_logic_vector(resize(mem_addr_latch,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if (mem_addr_latch = GOAL_MEMORY_MAX_ADDRESS) then + -- Set New Occupied Head + mem_occupied_head_next <= mem_addr_base; + -- DONE + mem_stage_next <= IDLE; + else + mem_addr_base_next <= mem_addr_latch; + mem_addr_latch_next <= mem_addr_base; + mem_cnt_next <= mem_cnt + 1; + end if; + end if; + -- SET Next Addr (Previous Slot) + when 8 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET; + mem_write_data <= std_logic_vector(resize(mem_addr_latch,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + -- DONE + mem_stage_next <= IDLE; + end if; + when others => + null; + end case; + when RESET_MEMORY => + case (mem_cnt) is + -- SET Goal State + when 0 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STATE_OFFSET; + mem_write_data(CDR_INT8_WIDTH-1 downto 0) <= GoalStatus_package.STATUS_UNKNOWN; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Result Index + when 1 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_RESULT_INDEX_OFFSET; + mem_write_data <= std_logic_vector(empty_head_res_ind); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Next Pointer + when 2 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET; + if (mem_addr_base = MAX_GOAL_ADDRESS) then + mem_write_data <= std_logic_vector(resize(GOAL_MEMORY_MAX_ADDRESS,WORD_WIDTH)); + else + mem_write_data <= std_logic_vector(resize(mem_addr_base + GOAL_FRAME_SIZE,WORD_WIDTH)); + end if; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Previous Pointer + when 3 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_PREV_ADDR_OFFSET; + mem_write_data <= std_logic_vector(resize(mem_addr_latch,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if (mem_addr_base = MAX_GOAL_ADDRESS) then + -- Initialize Empty and Occupied Heads + mem_empty_head_next <= FIRST_GOAL_ADDRESS; + mem_empty_tail_next <= MAX_GOAL_ADDRESS; + mem_occupied_head_next <= GOAL_MEMORY_MAX_ADDRESS; + mem_occupied_tail_next <= GOAL_MEMORY_MAX_ADDRESS; + empty_head_res_ind_next <= (others => '0'); + + -- DONE + mem_stage_next <= IDLE; + else + empty_head_res_ind_next <= empty_head_res_ind + 1; + mem_addr_latch_next <= mem_addr_base; + mem_addr_base_next <= mem_addr_base + GOAL_FRAME_SIZE; + mem_cnt_next <= 0; + end if; + end if; + when others => + null; + end case; + end case; + end process; + + sync_prc : process(clk) + begin + if rising_edge(clk) then + if (reset = '1') then + stage <= RESET_MEMORY; + mem_stage <= RESET_MEMORY; + mem_data_r <= ZERO_GOAL_DATA; + mem_latch_data <= ZERO_GOAL_DATA; + mem_data <= ZERO_GOAL_DATA; + rrq_addr_base <= FIRST_RRQ_ADDRESS; + rrq_addr_latch <= RRQ_MEMORY_MAX_ADDRESS; + mem_addr_base <= FIRST_GOAL_ADDRESS; + mem_addr_latch <= GOAL_MEMORY_MAX_ADDRESS; + rrq_empty_head <= RRQ_MEMORY_MAX_ADDRESS; + rrq_occupied_head <= RRQ_MEMORY_MAX_ADDRESS; + mem_occupied_head <= GOAL_MEMORY_MAX_ADDRESS; + mem_occupied_tail <= GOAL_MEMORY_MAX_ADDRESS; + mem_empty_head <= GOAL_MEMORY_MAX_ADDRESS; + mem_empty_tail <= GOAL_MEMORY_MAX_ADDRESS; + return_code_latch <= ROS_RET_OK; + request_id_latch <= EMPTY_REQUEST_ID; + check_time <= ROS_TIME_INFINITE; + time_latch <= ROS_TIME_INVALID; + goal_handle_latch <= GOAL_HANDLE_UNKNOWN; + goal_state_latch <= GoalStatus_package.STATUS_UNKNOWN; + g_stamp <= ROS_TIME_INVALID; + search_type_c <= SEARCH_NONE; + cancel_ret_code <= CancelGoal_package.RR_ERROR_NONE; + cnt <= 0; + terminal_cnt <= 0; + goals_canceling_cnt <= 0; + status_list_cnt <= 0; + mem_cnt <= 0; + trigger_result <= '0'; + trigger_status <= '1'; -- Trigger Status Update immediately + g_accept <= '0'; + empty_head_res_ind <= (others => '0'); + else + stage <= stage_next; + mem_stage <= mem_stage_next; + mem_data_r <= mem_data_r_next; + mem_latch_data <= mem_latch_data_next; + mem_data <= mem_data_next; + rrq_addr_base <= rrq_addr_base_next; + rrq_addr_latch <= rrq_addr_latch_next; + mem_addr_base <= mem_addr_base_next; + mem_addr_latch <= mem_addr_latch_next; + rrq_empty_head <= rrq_empty_head_next; + rrq_occupied_head <= rrq_occupied_head_next; + mem_occupied_head <= mem_occupied_head_next; + mem_occupied_tail <= mem_occupied_tail_next; + mem_empty_head <= mem_empty_head_next; + mem_empty_tail <= mem_empty_tail_next; + return_code_latch <= return_code_latch_next; + request_id_latch <= request_id_latch_next; + check_time <= check_time_next; + time_latch <= time_latch_next; + goal_handle_latch <= goal_handle_latch_next; + goal_state_latch <= goal_state_latch_next; + g_stamp <= g_stamp_next; + search_type_c <= search_type_c_next; + cancel_ret_code <= cancel_ret_code_next; + cnt <= cnt_next; + terminal_cnt <= terminal_cnt_next; + goals_canceling_cnt <= goals_canceling_cnt_next; + status_list_cnt <= status_list_cnt_next; + mem_cnt <= mem_cnt_next; + trigger_result <= trigger_result_next; + trigger_status <= trigger_status_next; + g_accept <= g_accept_next; + empty_head_res_ind <= empty_head_res_ind_next; + end if; + end if; + end process; + +end architecture; diff --git a/src/ros2/ros_action_server.vhd.MULTIPROCESS_BAK b/src/ros2/ros_action_server.vhd.MULTIPROCESS_BAK new file mode 100644 index 0000000..04ad276 --- /dev/null +++ b/src/ros2/ros_action_server.vhd.MULTIPROCESS_BAK @@ -0,0 +1,3413 @@ +-- 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.math_pkg.all; +use work.rtps_package.all; +use work.rtps_config_package.all; +use work.ros_package.all; +use work.Fibonacci_package.all; +use work.GoalStatus_package; +use work.CancelGoal_package; +use work.GoalStatusArray_package; + +entity ros_action_server is + generic ( + TIMEOUT_DURATION : ROS_DURATION_TYPE; + MAX_GOALS : natural; + MAX_RESULT_REQUESTS : natural + ); + port ( + -- SYSTEM + clk : in std_logic; + reset : in std_logic; + time : in ROS_TIME_TYPE; + -- *GOAL SERVICE* + start_g : out std_logic; + ack_g : in std_logic; + opcode_g : out ROS_SERVICE_OPCODE_TYPE; + service_info_g : in SERVICE_INFO_TYPE; + request_id_g : out REQUEST_ID_TYPE; + data_available_g : in std_logic; + taken_g : in std_logic; + done_g : in std_logic; + return_code_g : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + -- REQUEST + goal_id_g : in std_logic_vector(UUID_WIDTH-1 downto 0); + -- RESPONSE + accepted_g : out std_logic; + stamp_g : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + -- *RESULT SERVICE* + start_r : out std_logic; + ack_r : in std_logic; + opcode_r : out ROS_SERVICE_OPCODE_TYPE; + service_info_r : in SERVICE_INFO_TYPE; + request_id_r : out REQUEST_ID_TYPE; + data_available_r : in std_logic; + taken_r : in std_logic; + done_r : in std_logic; + return_code_r : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + result_index : out std_logic_vector(WORD_WIDTH-1 downto 0); + result_sel : out std_logic; + result_sel_ack : in std_logic; + -- REQUEST + goal_id_r : in std_logic_vector(UUID_WIDTH-1 downto 0); + -- RESPONSE + status_r : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + -- *CANCEL SERVICE* + start_c : out std_logic; + ack_c : in std_logic; + opcode_c : out ROS_SERVICE_OPCODE_TYPE; + service_info_c : in SERVICE_INFO_TYPE; + request_id_c : out REQUEST_ID_TYPE; + data_available_c : in std_logic; + taken_c : in std_logic; + done_c : in std_logic; + return_code_c : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + -- REQUEST + goal_info_goal_id_c : in std_logic_vector(UUID_WIDTH-1 downto 0); + goal_info_stamp_c : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + -- RESPONSE + cancel_return_code_c : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + goals_canceling_len_c : out std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0); + goals_canceling_addr_c : out std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0); + goals_canceling_ready_c : in std_logic; + goals_canceling_ren_c : out std_logic; + goals_canceling_wen_c : out std_logic; + goals_canceling_valid_c : in std_logic; + goals_canceling_ack_c : out std_logic; + goals_canceling_goal_id_r_c : in std_logic_vector(UUID_WIDTH-1 downto 0); + goals_canceling_goal_id_w_c : out std_logic_vector(UUID_WIDTH-1 downto 0); + goals_canceling_stamp_r_c : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + goals_canceling_stamp_w_c : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + -- *FEEDBACK TOPIC* + start_fb : out std_logic; + opcode_fb : out ROS_TOPIC_OPCODE_TYPE; + ack_fb : in std_logic; + done_fb : in std_logic; + return_code_fb : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + goal_id_fb : out std_logic_vector(UUID_WIDTH-1 downto 0); + -- *STATUS TOPIC* + start_s : out std_logic; + opcode_s : out ROS_TOPIC_OPCODE_TYPE; + ack_s : in std_logic; + done_s : in std_logic; + return_code_s : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + status_list_len_s : out std_logic_vector(GoalStatusArray_package.STATUS_LIST_ADDR_WIDTH-1 downto 0); + status_list_addr_s : out std_logic_vector(GoalStatusArray_package.STATUS_LIST_ADDR_WIDTH-1 downto 0); + status_list_ready_s : in std_logic; + status_list_ren_s : out std_logic; + status_list_wen_s : out std_logic; + status_list_valid_s : in std_logic; + status_list_ack_s : out std_logic; + status_list_goal_info_goal_id_r_s : in std_logic_vector(UUID_WIDTH-1 downto 0); + status_list_goal_info_goal_id_w_s : out std_logic_vector(UUID_WIDTH-1 downto 0); + status_list_goal_info_stamp_r_s : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + status_list_goal_info_stamp_w_s : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0); + status_list_status_r_s : in std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + status_list_status_w_s : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + -- *USER* + start : in std_logic; + opcode : in ROS_ACTION_OPCODE_TYPE; + ack : out std_logic; + done : out std_logic; + return_code : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + goal_handle_in : in std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); + goal_handle_out : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); + goal_state_in : in std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + goal_state_out : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + goal_id : out std_logic_vector(UUID_WIDTH-1 downto 0); + goal_result_index : out std_logic_vector(WORD_WIDTH-1 downto 0); + goal_stamp : out ROS_TIME_TYPE; + -- GOAL + new_goal_request : out std_logic; + new_goal_handle : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); + new_goal_result_index : out std_logic_vector(WORD_WIDTH-1 downto 0); + new_goal_accepted : in std_logic; + new_goal_response : in std_logic; + -- CANCEL + cancel_request : out std_logic; + cancel_request_handle : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); + cancel_accepted : in std_logic; + cancel_response : in std_logic + ); +end entity; + +architecture arch of ros_action_server is + + --*****CONSTANT DECLARATION***** + -- *GOAL MEMORY* + -- 4-Byte Word Size of a Goal Entry in Memory + function gen_goal_frame_size(TIMEOUT_DURATION : ROS_DURATION_TYPE) return natural is + begin + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then + return 12; + else + return 10; + end if; + end function; + constant GOAL_FRAME_SIZE : natural := gen_goal_frame_size(TIMEOUT_DURATION); + -- Goal Memory Size in 4-Byte Words + constant GOAL_MEMORY_SIZE : natural := MAX_GOALS * GOAL_FRAME_SIZE; + -- Goal Memory Address Width + constant GOAL_MEMORY_ADDR_WIDTH : natural := log2c(GOAL_MEMORY_SIZE); + -- Highest Goal Memory Address + constant GOAL_MEMORY_MAX_ADDRESS : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0) := to_unsigned(GOAL_MEMORY_SIZE-1, GOAL_MEMORY_ADDR_WIDTH); + -- Highest Goal Frame Address + constant MAX_GOAL_ADDRESS : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0) := GOAL_MEMORY_MAX_ADDRESS - GOAL_FRAME_SIZE + 1; + -- Address pointing to the beginning of the first Goal Data Frame + constant FIRST_GOAL_ADDRESS : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0'); + -- *RESULT REQUEST MEMORY* + -- 4-Byte Word Size of a Result Request Entry in Memory + constant RRQ_FRAME_SIZE : natural := 9; + -- Result Request Memory Size in 4-Byte Words + constant RRQ_MEMORY_SIZE : natural := MAX_RESULT_REQUESTS * RRQ_FRAME_SIZE; + -- Result Request Memory Address Width + constant RRQ_MEMORY_ADDR_WIDTH : natural := log2c(RRQ_MEMORY_SIZE); + -- Highest Result Request Memory Address + constant RRQ_MEMORY_MAX_ADDRESS : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0) := to_unsigned(RRQ_MEMORY_SIZE-1, RRQ_MEMORY_ADDR_WIDTH); + -- Highest Result Request Frame Address + constant MAX_RRQ_ADDRESS : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0) := RRQ_MEMORY_MAX_ADDRESS - RRQ_FRAME_SIZE + 1; + -- Address pointing to the beginning of the first Result Request Data Frame + constant FIRST_RRQ_ADDRESS : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0'); + + -- *GOAL MEMORY FRAME FIELD OFFSETS* + -- 4-Byte Word Offsets to Beginning of Respective Fields in the Goal Memory Frame + constant GMF_STATE_OFFSET : natural := 0; + constant GMF_GOAL_ID_OFFSET : natural := 1; + constant GMF_STAMP_OFFSET : natural := 5; + constant GMF_DEADLINE_OFFSET : natural := 7; + function gen_gmf_result_index_offset(TIMEOUT_DURATION : ROS_DURATION_TYPE) return natural is + variable ret : natural := 0; + begin + if (TIMEOUT_DURATION = ROS_DURATION_INFINITE) then + ret := GMF_DEADLINE_OFFSET + 2; + else + ret := GMF_DEADLINE_OFFSET; + end if; + return ret; + end function; + constant GMF_RESULT_INDEX_OFFSET : natural := gen_gmf_result_index_offset(TIMEOUT_DURATION); + constant GMF_NEXT_ADDR_OFFSET : natural := GMF_RESULT_INDEX_OFFSET + 1; + constant GMF_PREV_ADDR_OFFSET : natural := GMF_NEXT_ADDR_OFFSET + 1; + -- *RESULT REQUEST MEMORY FRAME FIELD OFFSETS* + -- 4-Byte Word Offsets to Beginning of Respective Fields in the Result Request Memory Frame + constant RMF_GOAL_HANDLE_OFFSET : natural := 0; + constant RMF_REQUEST_ID_OFFSET : natural := 1; + constant RMF_NEXT_ADDR_OFFSET : natural := 7; + constant RMF_PREV_ADDR_OFFSET : natural := 8; + + -- *GOAL MEMORY FRAME FIELD FLAGS* + -- Flags mapping to the respective Goal Memory Frame Fields + constant GMF_FLAG_WIDTH : natural := 7; + constant GMF_STATE_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (0 => '1', others => '0'); + constant GMF_GOAL_ID_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (1 => '1', others => '0'); + constant GMF_STAMP_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (2 => '1', others => '0'); + constant GMF_DEADLINE_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (3 => '1', others => '0'); + constant GMF_RESULT_INDEX_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (4 => '1', others => '0'); + constant GMF_NEXT_ADDR_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (5 => '1', others => '0'); + constant GMF_PREV_ADDR_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (6 => '1', others => '0'); + + --*****TYPE DECLARATION***** + -- *Cancel Process Search Types* + -- SEARCH_GOAL_ID Search goal list for a specific goal id + -- SEARCH_STAMP Search goal list for first goal that was accepted at or before a provided timestamp + -- SEARCH_NONE No search criteria. All goals are valid candites (Applies from current list position) + type SEARCH_TYPE is (SEARCH_GOAL_ID, SEARCH_STAMP, SEARCH_NONE); + -- *Instance Memory Opcodes* + -- OPCODE DESCRIPTION + -- INSERT Insert goal into memory + -- GET Get data of goal pointed by "GOAL_DATA_TYPE.addr" according to "GOAL_DATA_TYPE.field_flags". + -- Already fetched data of the goal is not modified. + -- GET_NEXT Get goal data of next goal (from the goal pointed by "GOAL_DATA_TYPE.addr") according to "GOAL_DATA_TYPE.field_flags". + -- Set "GOAL_DATA_TYPE.addr" to address of goal or GOAL_MEMORY_MAX_ADDRESS if no other goal in Memory. + -- GET_PREV Get goal data of previous goal (from the goal pointed by "GOAL_DATA_TYPE.addr") according to "GOAL_DATA_TYPE.field_flags". + -- Set "GOAL_DATA_TYPE.addr" to address of goal or GOAL_MEMORY_MAX_ADDRESS if no other goal in Memory. + -- UPDATE Update goal data pointed by "GOAL_DATA_TYPE.addr" according to "GOAL_DATA_TYPE.field_flags" + -- REMOVE Remove goal pointed by "GOAL_DATA_TYPE.addr" + type MEM_OPCODE_TYPE is (NOP, INSERT, GET, GET_NEXT, GET_PREV, UPDATE, REMOVE); + type STAGE_TYPE is (IDLE,WAIT_FOR_DATA,SEARCH_ID,CHECK_STATE,STORE_RRQ,SEND_RESPONSE,WAIT_FOR_RET,TRANSITION_STATE,GET_DATA,CHECK_REQUESTS,GET_REQUEST_ID,REMOVE_REQUEST,WAIT_FOR_MEM,RETURN_USER,CHECK_TIMEOUT,REMOVE_OLDEST,PUBLISH_FEEDBACK,RESET_MEMORY); + type STATUS_STAGE_TYPE is (IDLE,GET_FIRST,GET_NEXT,WAIT_FOR_DATA,PUSH_STATUS,PUBLISH,WAIT_FOR_PUBLISHER); + type GOAL_STAGE_TYPE is (IDLE,WAIT_FOR_DATA,WAIT_FOR_USER,ADD_GOAL,SEND_RESPONSE,WAIT_FOR_RET); + type CANCEL_STAGE_TYPE is (IDLE,WAIT_FOR_DATA,GET,GET_NEXT,CHECK,CHECK_GOAL_ID,CHECK_STAMP,CHECK_STATE,WAIT_FOR_USER,CANCEL_GOAL,ADD_CANCEL,SEND_RESPONSE,WAIT_FOR_RET); + type MEM_STAGE_TYPE is (IDLE,INSERT,GET,GET_NEXT,GET_PREV,UPDATE,REMOVE,RESET_MEMORY); + type GOAL_DATA_TYPE is record + addr : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); + goal_id : GUID_TYPE; + stamp : ROS_TIME_TYPE; + deadline : ROS_DURATION_TYPE; + res_ind : std_logic_vector(WORD_WIDTH-1 downto 0); + state : std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + field_flags : std_logic_vector(0 to GMF_FLAG_WIDTH-1); + end record; + constant ZERO_GOAL_DATA : GOAL_DATA_TYPE := ( + addr => (others => '0'), + goal_id => GUID_UNKNOWN, + stamp => ROS_TIME_ZERO, + deadline => ROS_DURATION_INFINITE, + res_ind => (others => '0'), + state => GoalStatus_package.STATUS_UNKNOWN, + field_flags => (others => '0') + ); + + --*****SIGNAL DECLARATION***** + -- *GOAL MEMORY CONNECTION SIGNALS* + signal mem_addr : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); + signal mem_read : std_logic; + signal mem_read_data, mem_write_data : std_logic_vector(WORD_WIDTH-1 downto 0); + signal mem_ready_in, mem_valid_in : std_logic; + signal mem_ready_out, mem_valid_out : std_logic; + signal mem_abort_read : std_logic; + -- *RESULT REQUEST MEMORY CONNECTION SIGNALS* + signal rrq_addr : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0); + signal rrq_read : std_logic; + signal rrq_read_data, rrq_write_data : std_logic_vector(WORD_WIDTH-1 downto 0); + signal rrq_ready_in, rrq_valid_in : std_logic; + signal rrq_ready_out, rrq_valid_out : std_logic; + signal rrq_abort_read : std_logic; + + -- *MAIN PROCESS* + -- FSM state + signal stage, stage_next : STAGE_TYPE; + -- Request memory operation + signal mem_start_r : std_logic; + -- Opcode of goal memory operation (valid only if mem_start_r is high) + signal mem_opcode_r : MEM_OPCODE_TYPE; + -- Signal used to pass data to Goal Memory Process + signal mem_r : GOAL_DATA_TYPE; + -- Goal memory data latch + signal mem_data_r, mem_data_r_next : GOAL_DATA_TYPE; + -- Head of occupied result requests list + signal rrq_occupied_head, rrq_occupied_head_next : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0); + -- Head of empty result requests list + signal rrq_empty_head, rrq_empty_head_next : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0); + -- General purpose counter + signal cnt, cnt_next : natural range 0 to 11; + -- Pointer to current relevant Result Request Memory Frame + signal rrq_addr_base, rrq_addr_base_next : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0); + -- Result Request Memory Frame pointer latch + signal rrq_addr_latch, rrq_addr_latch_next : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0); + -- Count of stored goals that are in a terminal state + signal terminal_cnt, terminal_cnt_next : natural range 0 to MAX_GOALS; + -- User return code latch + signal return_code_latch, return_code_latch_next : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); + -- Toggle latch used to trigger stored result request search when a goal is updated to a terminal state + signal trigger_result, trigger_result_next : std_logic; + -- Signal used to signal goal status change (Used by status process to generate new goal status list) + signal trigger_status_r : std_logic; + -- Result service Request ID latch + signal request_id_latch, request_id_latch_next : REQUEST_ID_TYPE; + -- Time of next goal expiration dealine + signal check_time, check_time_next : ROS_TIME_TYPE; + -- Temporal time latch (used in time addition calculation) + signal time_latch, time_latch_next : ROS_TIME_TYPE; + -- Signals that the main FSM has entered the atomic operation range (Used mutex lock between main and cancel FSMs) + signal atomic_op_r : std_logic; + -- Goal handle latch + signal goal_handle_latch, goal_handle_latch_next : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); + -- Goal state latch + signal goal_state_latch, goal_state_latch_next :std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + + -- *GOAL PROCESS* + -- Goal Process FSM state + signal goal_stage, goal_stage_next : GOAL_STAGE_TYPE; + -- Request memory operation + signal mem_start_g : std_logic; + -- Opcode of memory operation (valid only if mem_start_g is high) + signal mem_opcode_g : MEM_OPCODE_TYPE; + -- Goal timestamp latch + signal g_stamp, g_stamp_next : ROS_TIME_TYPE; + -- Goal accept latch + signal g_accept, g_accept_next : std_logic; + -- Signal used to signal goal status change (Used by status process to generate new goal status list) + signal trigger_status_g : std_logic; + + -- *CANCEL PROCESS* + -- Cancel process FSM state + signal cancel_stage, cancel_stage_next : CANCEL_STAGE_TYPE; + -- Request memory operation + signal mem_start_c : std_logic; + -- Opcode of memory operation (valid only if mem_start_c is high) + signal mem_opcode_c : MEM_OPCODE_TYPE; + -- Signal used to pass data to Goal Memory Process + signal mem_c : GOAL_DATA_TYPE; + -- General purpose counter + signal c_cnt, c_cnt_next : natural range 0 to 3; + -- Goal memory data latch + signal mem_data_c, mem_data_c_next : GOAL_DATA_TYPE; + -- Signals the kind of search the cancel FSM is doing (see SEARCH_TYPE definition) + signal search_type_c, search_type_c_next : SEARCH_TYPE; + -- Index of goals_canceling sequence + signal goals_canceling_cnt, goals_canceling_cnt_next : natural range 0 to CancelGoal_package.RR_GOALS_CANCELING_MAX_DEPTH-1; + -- Cancel response return code latch + signal cancel_ret_code, cancel_ret_code_next : std_logic_vector(CDR_INT8_WIDTH-1 downto 0); + -- Signal used to signal goal status change (Used by status process to generate new goal status list) + signal trigger_status_c : std_logic; + -- Signals that the cancel FSM has entered the atomic operation range (Used mutex lock between main and cancel FSMs) + signal atomic_op_c: std_logic; + + -- *STATUS PROCESS* + -- Status process FSM + signal status_stage, status_stage_next : STATUS_STAGE_TYPE; + -- Toggle latch that latches status update requests from other processes + signal trigger_status, trigger_status_next : std_logic; + -- Index of statusl list sequence + signal status_list_cnt, status_list_cnt_next : natural range 0 to MAX_GOALS-1; + -- Request memory operation + signal mem_start_s : std_logic; + -- Opcode of memory operation (valid only if mem_start_s is high) + signal mem_opcode_s : MEM_OPCODE_TYPE; + -- Signal used to pass data to Goal Memory Process + signal mem_s : GOAL_DATA_TYPE; + -- Goal memory data latch + signal mem_data_s, mem_data_s_next : GOAL_DATA_TYPE; + -- Test signal used for testbench synchronisation + signal status_idle_sig : std_logic; + + -- *GOAL MEMORY PROCESS* + -- Memory process FSM state + signal mem_stage, mem_stage_next : MEM_STAGE_TYPE; + -- Acknoledge memoryrequest from respective process + signal mem_ack_r, mem_ack_s, mem_ack_c, mem_ack_g : std_logic; + -- Head of occupied goal list + signal mem_occupied_head, mem_occupied_head_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); + -- Tail of occupied goal list + signal mem_occupied_tail, mem_occupied_tail_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); + -- Head of empty goal list + signal mem_empty_head, mem_empty_head_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); + -- Tail of empty goal list + signal mem_empty_tail, mem_empty_tail_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); + -- General purpose counter + signal mem_cnt, mem_cnt_next : natural range 0 to 19; + -- Pointer to currently relevant goal memory frame + signal mem_addr_base, mem_addr_base_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); + -- Goal memory fram pointer latch + signal mem_addr_latch, mem_addr_latch_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); + -- Result index of current empty head goal + signal empty_head_res_ind, empty_head_res_ind_next : unsigned(WORD_WIDTH-1 downto 0); + -- Latch for data from processes + signal mem_latch_data, mem_latch_data_next : GOAL_DATA_TYPE; + -- Latch for goal memory data + signal mem_data, mem_data_next : GOAL_DATA_TYPE; + -- Signals end of memory operation (And that the memory process is in the IDLE state) + signal mem_done : std_logic; + + -- *FUNCTION DECLARATION* + -- Returns true if the state is terminal + function is_terminal (state : std_logic_vector) return boolean is + begin + assert (state'length = CDR_INT8_WIDTH) severity FAILURE; + + case (state) is + when GoalStatus_package.STATUS_SUCCEEDED => + return TRUE; + when GoalStatus_package.STATUS_CANCELED => + return TRUE; + when GoalStatus_package.STATUS_ABORTED => + return TRUE; + when others => + return FALSE; + end case; + end function; + +begin + + goal_mem_ctrl_inst : entity work.mem_ctrl(arch) + generic map ( + ADDR_WIDTH => GOAL_MEMORY_ADDR_WIDTH, + DATA_WIDTH => WORD_WIDTH, + MEMORY_DEPTH => GOAL_MEMORY_SIZE, + MAX_BURST_LENGTH => GOAL_FRAME_SIZE + ) + port map ( + clk => clk, + reset => reset or mem_abort_read, + addr => std_logic_vector(mem_addr), + read => mem_read, + ready_in => mem_ready_in, + valid_in => mem_valid_in, + data_in => mem_write_data, + ready_out => mem_ready_out, + valid_out => mem_valid_out, + data_out => mem_read_data + ); + + rrq_mem_ctrl_inst : entity work.mem_ctrl(arch) + generic map ( + ADDR_WIDTH => RRQ_MEMORY_ADDR_WIDTH, + DATA_WIDTH => WORD_WIDTH, + MEMORY_DEPTH => RRQ_MEMORY_SIZE, + MAX_BURST_LENGTH => RRQ_FRAME_SIZE + ) + port map ( + clk => clk, + reset => reset or rrq_abort_read, + addr => std_logic_vector(rrq_addr), + read => rrq_read, + ready_in => rrq_ready_in, + valid_in => rrq_valid_in, + data_in => rrq_write_data, + ready_out => rrq_ready_out, + valid_out => rrq_valid_out, + data_out => rrq_read_data + ); + + result_index <= mem_data_r.res_ind; + status_r <= mem_data_r.state; + request_id_r <= request_id_latch; + goal_id_fb <= std_logic_vector(to_unsigned(mem_data_r.goal_id)); + goal_id <= std_logic_vector(to_unsigned(mem_data_r.goal_id)); + goal_stamp <= mem_data_r.stamp; + goal_state_out <= mem_data_r.state; + goal_result_index <= mem_data_r.res_ind; + request_id_g <= service_info_g.request_id; + accepted_g <= g_accept; + stamp_g <= std_logic_vector(to_unsigned(g_stamp)); + request_id_c <= service_info_c.request_id; + cancel_return_code_c <= cancel_ret_code; + goals_canceling_len_c <= std_logic_vector(to_unsigned(goals_canceling_cnt,goals_canceling_len_c'length)); + status_list_len_s <= std_logic_vector(to_unsigned(status_list_cnt,status_list_len_s'length)); + + -- *Main State Machine* + -- STATE DESCRIPTION + -- IDLE Idle state + -- WAIT_FOR_DATA Wait for result service request + -- SEARCH_ID Search for goal with result request goal id + -- CHECK_STATE Check if found goal is in terminal state + -- STORE_RRQ Store result request in memory + -- SEND_RESPONSE Send result service response + -- WAIT_FOR_RET Wait for result service response + -- TRANSITION_STATE Update goal state + -- GET_DATA Get goal data required for result response + -- CHECK_REQUESTS Search if updated goal is in stored result requests + -- GET_REQUEST_ID Get data from stored result request + -- REMOVE_REQUEST Remove stored result request + -- WAIT_FOR_MEM Wait for goal data from memory + -- RETURN_USER User return + -- CHECK_TIMEOUT Check and remove expired goals + -- REMOVE_OLDEST Remove oldest goal in terminal state + -- PUBLISH_FEEDBACK Publish feedback + -- RESET_MEMORY Reset result request memory to empty state + main_prc : process(all) + begin + -- DEFAULT + stage_next <= stage; + cnt_next <= cnt; + mem_data_r_next <= mem_data_r; + terminal_cnt_next <= terminal_cnt; + return_code_latch_next <= return_code_latch; + trigger_result_next <= trigger_result; + rrq_addr_base_next <= rrq_addr_base; + rrq_addr_latch_next <= rrq_addr_latch; + rrq_empty_head_next <= rrq_empty_head; + rrq_occupied_head_next <= rrq_occupied_head; + request_id_latch_next <= request_id_latch; + time_latch_next <= time_latch; + check_time_next <= check_time; + goal_handle_latch_next <= goal_handle_latch; + goal_state_latch_next <= goal_state_latch; + -- DEFAULT Unregistered + ack <= '0'; + done <= '0'; + start_fb <= '0'; + rrq_abort_read <= '0'; + result_sel <= '0'; + atomic_op_r <= '0'; + trigger_status_r <= '0'; + mem_start_r <= '0'; + mem_opcode_r <= NOP; + start_r <= '0'; + opcode_r <= NOP; + opcode_fb <= NOP; + mem_r <= ZERO_GOAL_DATA; + return_code <= ROS_RET_OK; + goal_handle_out <= GOAL_HANDLE_UNKNOWN; + + + case (stage) is + when IDLE => + if (start = '1') then + case (opcode) is + when GET_GOAL => + mem_start_r <= '1'; + mem_opcode_r <= GET; + mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG; + mem_r.addr <= unsigned(goal_handle_in); + + if (mem_ack_r = '1') then + ack <= '1'; + stage_next <= WAIT_FOR_MEM; + return_code_latch_next <= ROS_RET_OK; + end if; + when GET_LAST_GOAL => + -- No Goals Available + if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then + mem_data_r_next.addr <= GOAL_MEMORY_MAX_ADDRESS; + ack <= '1'; + return_code_latch_next <= ROS_RET_OK; + stage_next <= RETURN_USER; + else + mem_start_r <= '1'; + mem_opcode_r <= GET; + mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG; + mem_r.addr <= mem_occupied_head; + + if (mem_ack_r = '1') then + ack <= '1'; + stage_next <= WAIT_FOR_MEM; + return_code_latch_next <= ROS_RET_OK; + end if; + end if; + when GET_PREVIOUS_GOAL => + mem_start_r <= '1'; + mem_opcode_r <= GET_NEXT; + mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG; + mem_r.addr <= unsigned(goal_handle_in); + + if (mem_ack_r = '1') then + ack <= '1'; + stage_next <= WAIT_FOR_MEM; + return_code_latch_next <= ROS_RET_OK; + end if; + when UPDATE_GOAL_STATE => + -- Atomic Operation Guard + if (atomic_op_c = '1' and unsigned(goal_handle_in) = mem_data_c.addr) then + -- NOTE: In order to prevent a user side Deadlock, we return with INVALID_ARGUMENT if the cancel FSM is also waiting for a user input in the atomic operation zone. + if (cancel_stage = WAIT_FOR_USER) then + ack <= '1'; + stage_next <= RETURN_USER; + return_code_latch_next <= ROS_RET_INVALID_ARGUMENT; + end if; + else + mem_start_r <= '1'; + mem_opcode_r <= GET; + mem_r.field_flags <= GMF_STATE_FLAG; + mem_r.addr <= unsigned(goal_handle_in); + + if (mem_ack_r = '1') then + -- Latch Input Data + goal_handle_latch_next <= goal_handle_in; + goal_state_latch_next <= goal_state_in; + + ack <= '1'; + stage_next <= TRANSITION_STATE; + cnt_next <= 0; + end if; + end if; + when PUBLISH_FEEDBACK => + mem_start_r <= '1'; + mem_opcode_r <= GET_NEXT; + mem_r.field_flags <= GMF_GOAL_ID_FLAG; + mem_r.addr <= unsigned(goal_handle_in); + + if (mem_ack_r = '1') then + ack <= '1'; + stage_next <= PUBLISH_FEEDBACK; + cnt_next <= 0; + end if; + when others => + null; + end case; + elsif (data_available_r = '1' and rrq_empty_head /= RRQ_MEMORY_MAX_ADDRESS) then + start_r <= '1'; + opcode_r <= TAKE_REQUEST; + + if (ack_r = '1') then + stage_next <= WAIT_FOR_DATA; + end if; + elsif (TIMEOUT_DURATION /= ROS_TIME_INFINITE and time >= check_time) then + assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + -- Reset + check_time_next <= ROS_TIME_INFINITE; + + mem_data_r_next.addr <= mem_occupied_head; + stage_next <= CHECK_TIMEOUT; + cnt_next <= 0; + elsif (TIMEOUT_DURATION = ROS_TIME_INFINITE and mem_empty_head = GOAL_MEMORY_MAX_ADDRESS and terminal_cnt /= 0) then + assert (mem_occupied_tail /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + mem_data_r_next.addr <= mem_occupied_tail; + stage_next <= REMOVE_OLDEST; + cnt_next <= 0; + end if; + when WAIT_FOR_DATA => + if (done_r = '1') then + case (return_code_r) is + when ROS_RET_OK => + if (taken_r = '1') then + -- Latch Request ID + request_id_latch_next <= service_info_r.request_id; + + -- No Goals Available + if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then + mem_data_r_next.res_ind <= std_logic_vector(to_unsigned(MAX_GOALS, WORD_WIDTH)); + mem_data_r_next.state <= GoalStatus_package.STATUS_UNKNOWN; + stage_next <= SEND_RESPONSE; + else + stage_next <= SEARCH_ID; + cnt_next <= 0; + end if; + else + stage_next <= IDLE; + end if; + when others => + -- TODO: Propagate Error? + stage_next <= IDLE; + end case; + end if; + when SEARCH_ID => + case (cnt) is + -- GET FIRST + when 0 => + assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + mem_start_r <= '1'; + mem_opcode_r <= GET; + mem_r.field_flags <= GMF_GOAL_ID_FLAG; + mem_r.addr <= mem_occupied_head; + + if (mem_ack_r = '1') then + cnt_next <= cnt + 2; + end if; + -- GET NEXT + when 1 => + mem_start_r <= '1'; + mem_opcode_r <= GET_NEXT; + mem_r.field_flags <= GMF_GOAL_ID_FLAG; + mem_r.addr <= mem_data_r.addr; + + if (mem_ack_r = '1') then + cnt_next <= cnt + 1; + end if; + -- CHECK + when 2 => + -- Wait for Memory + if (mem_done = '1') then + assert check_mask(mem_data.field_flags, GMF_GOAL_ID_FLAG) severity FAILURE; + + -- No more Goals + if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then + -- Goal not found + mem_data_r_next.res_ind <= std_logic_vector(to_unsigned(MAX_GOALS, WORD_WIDTH)); + mem_data_r_next.state <= GoalStatus_package.STATUS_UNKNOWN; + stage_next <= SEND_RESPONSE; + else + mem_data_r_next <= mem_data; + cnt_next <= cnt + 1; + end if; + end if; + -- CHECK GOAL_ID 1/4 + when 3 => + if (mem_data_r.goal_id(0) = to_GUID(goal_id_r)(0)) then + cnt_next <= cnt + 1; + else + cnt_next <= 1; -- GET_NEXT + end if; + -- CHECK GOAL_ID 2/4 + when 4 => + if (mem_data_r.goal_id(1) = to_GUID(goal_id_r)(1)) then + cnt_next <= cnt + 1; + else + cnt_next <= 1; -- GET_NEXT + end if; + -- CHECK GOAL_ID 3/4 + when 5 => + if (mem_data_r.goal_id(2) = to_GUID(goal_id_r)(2)) then + cnt_next <= cnt + 1; + else + cnt_next <= 1; -- GET_NEXT + end if; + -- CHECK GOAL_ID 4/4 + when 6 => + if (mem_data_r.goal_id(3) = to_GUID(goal_id_r)(3)) then + -- Found Goal + stage_next <= CHECK_STATE; + cnt_next <= 0; + else + cnt_next <= 1; -- GET_NEXT + end if; + when others => + null; + end case; + when CHECK_STATE => + case (cnt) is + -- GET STATE + when 0 => + mem_start_r <= '1'; + mem_opcode_r <= GET; + mem_r.field_flags <= GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG; + mem_r.addr <= mem_data_r.addr; + + if (mem_ack_r = '1') then + cnt_next <= cnt + 1; + end if; + -- Check State + when 1 => + -- Wait for Memory + if (mem_done = '1') then + assert (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + assert check_mask(mem_data.field_flags, GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG) severity FAILURE; + + mem_data_r_next <= mem_data; + + if (is_terminal(mem_data.state)) then + if (mem_data.state /= GoalStatus_package.STATUS_SUCCEEDED) then + -- NOTE: We are setting result index to MAX_GOALS to effectively "zero" the result. + -- This prevents sending the result of previous succeeded goals, if the current + -- goal is not succeeded. + mem_data_r_next.res_ind <= std_logic_vector(to_unsigned(MAX_GOALS, WORD_WIDTH)); + end if; + stage_next <= SEND_RESPONSE; + else + stage_next <= STORE_RRQ; + cnt_next <= 0; + end if; + end if; + when others => + null; + end case; + when STORE_RRQ => + assert (rrq_empty_head /= RRQ_MEMORY_MAX_ADDRESS) severity FAILURE; + + case (cnt) is + -- GET Next Addr + when 0 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_empty_head + RMF_NEXT_ADDR_OFFSET; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- SET Goal Handle + when 1 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_empty_head + RMF_GOAL_HANDLE_OFFSET; + rrq_write_data <= std_logic_vector(resize(mem_data_r.addr,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- SET Request ID 1/6 + when 2 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET; + rrq_write_data <= service_info_c.request_id.writer_guid(0); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- SET Request ID 2/6 + when 3 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 1; + rrq_write_data <= service_info_c.request_id.writer_guid(1); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- SET Request ID 3/6 + when 4 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 2; + rrq_write_data <= service_info_c.request_id.writer_guid(2); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- SET Request ID 4/6 + when 5 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 3; + rrq_write_data <= service_info_c.request_id.writer_guid(3); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- SET Request ID 5/6 + when 6 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 4; + rrq_write_data <= std_logic_vector(service_info_c.request_id.sequence_number(0)); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- SET Request ID 6/6 + when 7 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 5; + rrq_write_data <= std_logic_vector(service_info_c.request_id.sequence_number(1)); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + if (rrq_occupied_head = RRQ_MEMORY_MAX_ADDRESS) then + cnt_next <= cnt + 2; + else + cnt_next <= cnt + 1; + end if; + end if; + -- SET Prev Addr (Old Occupied Head) + when 8 => + assert (rrq_occupied_head /= RRQ_MEMORY_MAX_ADDRESS) severity FAILURE; + + rrq_valid_in <= '1'; + rrq_addr <= rrq_occupied_head + RMF_PREV_ADDR_OFFSET; + rrq_write_data <= std_logic_vector(resize(rrq_empty_head,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- SET Next Addr + when 9 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_empty_head + RMF_NEXT_ADDR_OFFSET; + rrq_write_data <= std_logic_vector(resize(rrq_occupied_head,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + -- Set New Occupied Head + rrq_occupied_head_next <= rrq_empty_head; + cnt_next <= cnt + 1; + end if; + -- SET Prev Addr + when 10 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_empty_head + RMF_PREV_ADDR_OFFSET; + rrq_write_data <= std_logic_vector(resize(RRQ_MEMORY_MAX_ADDRESS,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- READ Next Addr + when 11 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + -- Set new Empty Head + rrq_empty_head_next <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH); + -- DONE + stage_next <= IDLE; + end if; + when others => + null; + end case; + when SEND_RESPONSE => + assert check_mask(mem_data_r.field_flags, GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG) severity FAILURE; + result_sel <= '1'; + + if (result_sel_ack = '1') then + start_r <= '1'; + opcode_r <= SEND_RESPONSE; + + if (ack_r = '1') then + stage_next <= WAIT_FOR_RET; + end if; + end if; + when WAIT_FOR_RET => + assert check_mask(mem_data_r.field_flags, GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG) severity FAILURE; + result_sel <= '1'; + + if (done_r = '1') then + -- TODO: Propagate Error? + stage_next <= IDLE; + + if (trigger_result = '1') then + stage_next <= CHECK_REQUESTS; + cnt_next <= 0; + end if; + end if; + when TRANSITION_STATE => + assert (not (atomic_op_c = '1' and mem_data_r.addr = mem_data_c.addr)) severity FAILURE; + atomic_op_r <= '1'; + + case (cnt) is + -- CHECK Goal State + when 0 => + -- Wait for Memory + if (mem_done = '1') then + assert check_mask(mem_data.field_flags, GMF_STATE_FLAG) severity FAILURE; + + case (mem_data.state) is + when GoalStatus_package.STATUS_ACCEPTED => + case (goal_state_latch) is + when GoalStatus_package.STATUS_EXECUTING => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_TIME_INFINITE and is_terminal(goal_state_in)) then + cnt_next <= cnt + 1; + else + cnt_next <= cnt + 3; + end if; + when others => + ack <= '1'; + stage_next <= RETURN_USER; + return_code_latch_next <= ROS_RET_ACTION_GOAL_EVENT_INVALID; + end case; + when GoalStatus_package.STATUS_EXECUTING => + case (goal_state_latch) is + when GoalStatus_package.STATUS_SUCCEEDED => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_TIME_INFINITE and is_terminal(goal_state_in)) then + cnt_next <= cnt + 1; + else + cnt_next <= cnt + 3; + end if; + when GoalStatus_package.STATUS_ABORTED => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_TIME_INFINITE and is_terminal(goal_state_in)) then + cnt_next <= cnt + 1; + else + cnt_next <= cnt + 3; + end if; + when others => + ack <= '1'; + stage_next <= RETURN_USER; + return_code_latch_next <= ROS_RET_ACTION_GOAL_EVENT_INVALID; + end case; + when GoalStatus_package.STATUS_CANCELING => + case (goal_state_latch) is + when GoalStatus_package.STATUS_SUCCEEDED => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_TIME_INFINITE and is_terminal(goal_state_in)) then + cnt_next <= cnt + 1; + else + cnt_next <= cnt + 3; + end if; + when GoalStatus_package.STATUS_ABORTED => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_TIME_INFINITE and is_terminal(goal_state_in)) then + cnt_next <= cnt + 1; + else + cnt_next <= cnt + 3; + end if; + when GoalStatus_package.STATUS_CANCELED => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_TIME_INFINITE and is_terminal(goal_state_in)) then + cnt_next <= cnt + 1; + else + cnt_next <= cnt + 3; + end if; + when others => + ack <= '1'; + stage_next <= RETURN_USER; + return_code_latch_next <= ROS_RET_ACTION_GOAL_EVENT_INVALID; + end case; + when others => + ack <= '1'; + stage_next <= RETURN_USER; + return_code_latch_next <= ROS_RET_ACTION_GOAL_EVENT_INVALID; + end case; + end if; + -- Time Addition 1/2 + when 1 => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_TIME_INFINITE) then + time_latch_next.sec <= std_logic_vector(unsigned(time.sec) + unsigned(TIMEOUT_DURATION.sec)); + time_latch_next.nanosec <= std_logic_vector(unsigned(time.nanosec) + unsigned(TIMEOUT_DURATION.nanosec)); + cnt_next <= cnt + 1; + end if; + -- Time Addition 2/2 + when 2 => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_TIME_INFINITE) then + if (unsigned(time_latch.nanosec) >= to_unsigned(10**9,time_latch.nanosec'length)) then + time_latch_next.sec <= std_logic_vector(unsigned(time_latch.sec) + 1); + time_latch_next.nanosec <= std_logic_vector(unsigned(time_latch.nanosec) - to_unsigned(10**9,time_latch.nanosec'length)); + cnt_next <= cnt + 1; + end if; + end if; + -- UPDATE Goal State + when 3 => + mem_start_r <= '1'; + mem_opcode_r <= UPDATE; + mem_r.field_flags <= GMF_STATE_FLAG; + mem_r.addr <= unsigned(goal_handle_latch); + mem_r.state <= goal_state_latch; + + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_TIME_INFINITE and is_terminal(goal_state_in)) then + mem_r.field_flags <= GMF_STATE_FLAG or GMF_DEADLINE_FLAG; + mem_r.deadline <= time_latch; + + -- Update Checktime + if (time_latch < check_time) then + check_time_next <= time_latch; + end if; + end if; + + if (mem_ack_r = '1') then + if (is_terminal(goal_state_in)) then + trigger_result_next <= '1'; + -- Update Terminal Goal Counter + terminal_cnt_next <= terminal_cnt + 1; + end if; + ack <= '1'; + stage_next <= RETURN_USER; + return_code_latch_next <= ROS_RET_OK; + -- Trigger Status Update + trigger_status_r <= '1'; + end if; + when others => + null; + end case; + when GET_DATA => + case (cnt) is + -- GET Data + when 0 => + mem_start_r <= '1'; + mem_opcode_r <= GET; + mem_r.field_flags <= GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG; + mem_r.addr <= mem_data_r.addr; + + if (mem_ack_r = '1') then + cnt_next <= cnt + 1; + end if; + -- READ Data + when 1 => + -- Wait for Memory + if (mem_done = '1') then + assert (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + assert check_mask(mem_data.field_flags, GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG) severity FAILURE; + assert is_terminal(mem_data.state) severity FAILURE; + + mem_data_r_next <= mem_data; + + if (mem_data.state /= GoalStatus_package.STATUS_SUCCEEDED) then + -- NOTE: We are setting result index to MAX_GOALS to effectively "zero" the result. + -- This prevents sending the result of previous succeeded goals, if the current + -- goal is not succeeded. + mem_data_r_next.res_ind <= std_logic_vector(to_unsigned(MAX_GOALS, WORD_WIDTH)); + end if; + + stage_next <= CHECK_REQUESTS; + cnt_next <= 0; + end if; + when others => + null; + end case; + when CHECK_REQUESTS => + case (cnt) is + -- GET Goal Handle + when 0 => + -- NOTE: We check the base here, because a REMOVE_REQUEST operation may reset the rrq_addr_base + if (rrq_addr_base = RRQ_MEMORY_MAX_ADDRESS) then + -- Reset + trigger_result_next <= '0'; + + -- DONE + stage_next <= IDLE; + else + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_GOAL_HANDLE_OFFSET; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + end if; + -- GET Next Addr + when 1 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- READ Goal Handle + when 2 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + + -- Found Result Request of Updated Goal + if (resize(unsigned(rrq_read_data),GOAL_MEMORY_ADDR_WIDTH) = mem_data_r.addr) then + stage_next <= GET_REQUEST_ID; + cnt_next <= 0; + rrq_abort_read <= '1'; + else + cnt_next <= cnt + 1; + end if; + end if; + -- READ Next Addr + when 3 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + rrq_addr_base_next <= resize(unsigned(rrq_read_data),GOAL_MEMORY_ADDR_WIDTH); + + -- No More Stored Requests + if (resize(unsigned(rrq_read_data),GOAL_MEMORY_ADDR_WIDTH) = RRQ_MEMORY_MAX_ADDRESS) then + -- Reset + trigger_result_next <= '0'; + + stage_next <= IDLE; + else + -- Next Request + cnt_next <= 0; + end if; + end if; + when others => + null; + end case; + when GET_REQUEST_ID => + case (cnt) is + -- GET Request ID 1/6 + when 0 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- GET Request ID 2/6 + when 1 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 1; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- GET Request ID 3/6 + when 2 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 2; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- GET Request ID 4/6 + when 3 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 3; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- GET Request ID 5/6 + when 4 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 4; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- GET Request ID 6/6 + when 5 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 5; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- READ Request ID 1/6 + when 6 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + request_id_latch_next.writer_guid(0) <= rrq_read_data; + + cnt_next <= cnt + 1; + end if; + -- READ Request ID 2/6 + when 7 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + request_id_latch_next.writer_guid(1) <= rrq_read_data; + + cnt_next <= cnt + 1; + end if; + -- READ Request ID 3/6 + when 8 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + request_id_latch_next.writer_guid(2) <= rrq_read_data; + + cnt_next <= cnt + 1; + end if; + -- READ Request ID 4/6 + when 9 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + request_id_latch_next.writer_guid(3) <= rrq_read_data; + + cnt_next <= cnt + 1; + end if; + -- READ Request ID 5/6 + when 10 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + request_id_latch_next.sequence_number(0) <= unsigned(rrq_read_data); + + cnt_next <= cnt + 1; + end if; + -- READ Request ID 6/6 + when 11 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + request_id_latch_next.sequence_number(1) <= unsigned(rrq_read_data); + + stage_next <= REMOVE_REQUEST; + cnt_next <= 0; + end if; + when others => + null; + end case; + when REMOVE_REQUEST => + -- NOTE: After thsi stage the rrq_addr_base is set to the Next request + case (cnt) is + -- GET Next Addr + when 0 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- GET Prev Addr + when 1 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_PREV_ADDR_OFFSET; + rrq_read <= '1'; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- SET Next Addr + when 2 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET; + rrq_write_data <= std_logic_vector(resize(rrq_empty_head,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + -- Set New Empty Head + rrq_empty_head_next <= rrq_addr_base; + + cnt_next <= cnt + 1; + end if; + -- READ Next Addr + when 3 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + rrq_addr_latch_next <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH); + cnt_next <= cnt + 1; + end if; + -- READ Previous Addr + when 4 => + rrq_ready_out <= '1'; + + -- Memory Flow Control Guard + if (rrq_valid_out = '1') then + if (rrq_addr_latch = RRQ_MEMORY_MAX_ADDRESS) then + if (resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH) = RRQ_MEMORY_MAX_ADDRESS) then + -- Reset Occupied Head + rrq_occupied_head_next <= RRQ_MEMORY_MAX_ADDRESS; + + rrq_addr_base_next <= RRQ_MEMORY_MAX_ADDRESS; + stage_next <= SEND_RESPONSE; + else + rrq_addr_base_next <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH); + cnt_next <= cnt + 2; + end if; + else + rrq_addr_base_next <= rrq_addr_latch; + rrq_addr_latch_next <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH); + cnt_next <= cnt + 1; + end if; + end if; + -- SET Previous Addr (Next Slot) + when 6 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_PREV_ADDR_OFFSET; + rrq_write_data <= std_logic_vector(rrq_addr_latch); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + if (rrq_addr_latch = RRQ_MEMORY_MAX_ADDRESS) then + -- Set New Occupied Head + rrq_occupied_head_next <= rrq_addr_base; + + stage_next <= SEND_RESPONSE; + else + rrq_addr_base_next <= rrq_addr_latch; + rrq_addr_latch_next <= rrq_addr_base; + cnt_next <= cnt + 1; + end if; + end if; + -- SET Next Addr (Previous Slot) + when 7 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET; + rrq_write_data <= std_logic_vector(rrq_addr_latch); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + + rrq_addr_base_next <= rrq_addr_latch; + stage_next <= SEND_RESPONSE; + end if; + when others => + null; + end case; + when WAIT_FOR_MEM => + -- Wait for Memory + if (mem_done = '1') then + assert check_mask(mem_data.field_flags, GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG) severity FAILURE; + + mem_data_r_next <= mem_data; + + stage_next <= RETURN_USER; + end if; + when RETURN_USER => + -- End of Memory + if (mem_data_r.addr = GOAL_MEMORY_MAX_ADDRESS) then + goal_handle_out <= GOAL_HANDLE_UNKNOWN; + else + goal_handle_out <= std_logic_vector(resize(mem_data_r.addr, GOAL_HANDLE_WIDTH)); + end if; + + done <= '1'; + return_code <= return_code_latch; + + if (trigger_result = '1') then + stage_next <= GET_DATA; + cnt_next <= 0; + else + stage_next <= IDLE; + end if; + when CHECK_TIMEOUT => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_TIME_INFINITE) then + case (cnt) is + -- GET + when 0 => + mem_start_r <= '1'; + mem_opcode_r <= GET; + mem_r.field_flags <= GMF_DEADLINE_FLAG; + mem_r.addr <= mem_data_r.addr; + + if (mem_ack_r = '1') then + cnt_next <= cnt + 2; + end if; + -- GET NEXT + when 1 => + mem_start_r <= '1'; + mem_opcode_r <= GET_NEXT; + mem_r.field_flags <= GMF_DEADLINE_FLAG; + mem_r.addr <= mem_data_r.addr; + + if (mem_ack_r = '1') then + cnt_next <= cnt + 1; + end if; + -- CHECK + when 2 => + -- Wait for Memory + if (mem_done = '1') then + assert check_mask(mem_data.field_flags, GMF_DEADLINE_FLAG) severity FAILURE; + + mem_data_r_next <= mem_data; + + -- No more Goals + if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then + -- DONE + stage_next <= IDLE; + else + -- Timeout + if (mem_data.deadline <= time) then + cnt_next <= cnt + 1; + else + -- Update check Time + if (mem_data.deadline < check_time) then + check_time_next <= mem_data.deadline; + end if; + cnt_next <= 1; + end if; + end if; + end if; + -- REMOVE + when 3 => + -- TODO: This needs to take into account the other FSMs (Status, Cancel, Goal) + mem_start_r <= '1'; + mem_opcode_r <= REMOVE; + mem_r.addr <= mem_data_r.addr; + + if (mem_ack_r = '1') then + cnt_next <= cnt + 1; + -- Trigger Status Update + trigger_status_r <= '1'; + -- Update Terminal Goal Counter + terminal_cnt_next <= terminal_cnt - 1; + end if; + -- Wait for Remove + when 4 => + -- Wait for Memory + if (mem_done = '1') then + mem_data_r_next <= mem_data; + + -- No more Goals + if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then + if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then + -- DONE + stage_next <= IDLE; + else + mem_data_r.addr <= mem_occupied_head; + cnt_next <= 1; + end if; + else + cnt_next <= 1; + end if; + end if; + when others => + null; + end case; + end if; + when REMOVE_OLDEST => + -- Synthesis Guard + if (TIMEOUT_DURATION = ROS_TIME_INFINITE) then + case (cnt) is + -- GET + when 0 => + mem_start_r <= '1'; + mem_opcode_r <= GET; + mem_r.field_flags <= GMF_STATE_FLAG; + mem_r.addr <= mem_data_r.addr; + + if (mem_ack_r = '1') then + cnt_next <= cnt + 2; + end if; + -- GET NEXT + when 1 => + mem_start_r <= '1'; + mem_opcode_r <= GET_PREV; + mem_r.field_flags <= GMF_STATE_FLAG; + mem_r.addr <= mem_data_r.addr; + + if (mem_ack_r = '1') then + cnt_next <= cnt + 1; + end if; + -- CHECK + when 2 => + -- Wait for Memory + if (mem_done = '1') then + assert check_mask(mem_data.field_flags, GMF_STATE_FLAG) severity FAILURE; + + mem_data_r_next <= mem_data; + + -- No more Goals + if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then + -- DONE + stage_next <= IDLE; + else + if (is_terminal(mem_data.state)) then + cnt_next <= cnt + 1; + else + cnt_next <= 1; + end if; + end if; + end if; + -- REMOVE + when 3 => + mem_start_r <= '1'; + mem_opcode_r <= REMOVE; + mem_r.addr <= mem_data_r.addr; + + if (mem_ack_r = '1') then + cnt_next <= cnt + 1; + -- Trigger Status Update + trigger_status_r <= '1'; + -- Update Terminal Goal Counter + terminal_cnt_next <= terminal_cnt - 1; + end if; + -- Wait for Remove + when 4 => + -- Wait for Memory + if (mem_done = '1') then + mem_data_r_next <= mem_data; + + -- No more Goals + if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then + -- DONE + stage_next <= IDLE; + else + -- NOTE: After Removal, mem_data.addr is set to the previous goal, so we need to call GET and not GET_PREV + cnt_next <= 0; + end if; + end if; + when others => + null; + end case; + end if; + when PUBLISH_FEEDBACK => + case (cnt) is + -- Wait for Data + when 0 => + if (mem_done = '1') then + assert check_mask(mem_data.field_flags, GMF_GOAL_ID_FLAG) severity FAILURE; + + mem_data_r_next <= mem_data_r; + cnt_next <= cnt + 1; + end if; + -- Start Publish + when 1 => + start_fb <= '1'; + opcode_fb <= PUBLISH; + + if (ack_fb = '1') then + cnt_next <= cnt + 1; + end if; + -- Wait for Publish + when 2 => + -- Passthrough + done <= done_fb; + return_code <= return_code_fb; + + if (done_fb = '1') then + -- DONE + stage_next <= IDLE; + end if; + when others => + null; + end case; + when RESET_MEMORY => + case (cnt) is + -- SET Next Pointer + when 0 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET; + if (rrq_addr_base = MAX_RRQ_ADDRESS) then + rrq_write_data <= std_logic_vector(resize(RRQ_MEMORY_MAX_ADDRESS,WORD_WIDTH)); + else + rrq_write_data <= std_logic_vector(resize(rrq_addr_base + RRQ_FRAME_SIZE,WORD_WIDTH)); + end if; + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + cnt_next <= cnt + 1; + end if; + -- SET Previous Pointer + when 1 => + rrq_valid_in <= '1'; + rrq_addr <= rrq_addr_base + RMF_PREV_ADDR_OFFSET; + rrq_write_data <= std_logic_vector(resize(rrq_addr_latch,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (rrq_ready_in = '1') then + if (rrq_addr_base = MAX_RRQ_ADDRESS) then + -- Initialize Empty and Occupied Heads + rrq_empty_head <= FIRST_RRQ_ADDRESS; + rrq_occupied_head <= RRQ_MEMORY_MAX_ADDRESS; + + -- DONE + stage_next <= IDLE; + else + rrq_addr_latch_next <= rrq_addr_base; + rrq_addr_base_next <= rrq_addr_base + RRQ_FRAME_SIZE; + cnt_next <= 0; + end if; + end if; + when others => + null; + end case; + end case; + end process; + + -- *Goal State Machine* + -- STATE DESCRIPTION + -- IDLE Idle state + -- WAIT_FOR_DATA Wait goal service request + -- WAIT_FOR_USER Wait for user input + -- ADD_GOAL Add goal to memory + -- SEND_RESPONSE Send goal service response + -- WAIT_FOR_RET Wait for goal service response + goal_prc : process(all) + begin + -- DEFAULT + goal_stage_next <= goal_stage; + g_stamp_next <= g_stamp; + g_accept_next <= g_accept; + -- DEFAULT Unregistered + trigger_status_g <= '0'; + mem_start_g <= '0'; + mem_opcode_g <= NOP; + start_g <= '0'; + opcode_g <= NOP; + new_goal_request <= '0'; + new_goal_handle <= GOAL_HANDLE_UNKNOWN; + new_goal_result_index <= (others => '0'); + + case (goal_stage) is + when IDLE => + if (data_available_g = '1' and mem_empty_head /= GOAL_MEMORY_MAX_ADDRESS) then + start_g <= '1'; + opcode_g <= TAKE_REQUEST; + if (ack_g = '1') then + goal_stage_next <= WAIT_FOR_DATA; + end if; + end if; + when WAIT_FOR_DATA => + if (done_g = '1') then + case (return_code_g) is + when ROS_RET_OK => + if (taken_g = '1') then + goal_stage_next <= WAIT_FOR_USER; + else + goal_stage_next <= IDLE; + end if; + when others => + -- TODO: Propagate Error? + goal_stage_next <= IDLE; + end case; + end if; + when WAIT_FOR_USER => + new_goal_request <= '1'; + new_goal_handle <= std_logic_vector(resize(mem_empty_head,GOAL_HANDLE_WIDTH)); + new_goal_result_index <= std_logic_vector(empty_head_res_ind); + if (new_goal_response = '1') then + g_stamp_next <= time; + if (new_goal_accepted = '1') then + goal_stage_next <= ADD_GOAL; + g_accept_next <= '1'; + else + goal_stage_next <= SEND_RESPONSE; + g_accept_next <= '0'; + end if; + end if; + when ADD_GOAL => + assert (mem_empty_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + mem_start_g <= '1'; + mem_opcode_g <= INSERT; + + if (mem_ack_g = '1') then + goal_stage_next <= SEND_RESPONSE; + -- Trigger Status Update + trigger_status_g <= '1'; + end if; + when SEND_RESPONSE => + start_g <= '1'; + opcode_g <= SEND_RESPONSE; + if (ack_g = '1') then + goal_stage_next <= WAIT_FOR_RET; + end if; + when WAIT_FOR_RET => + if (done_g = '1') then + -- TODO: Propagate Error? + goal_stage_next <= IDLE; + end if; + end case; + end process; + + -- *Cancel State Machine* + -- STATE DESCRIPTION + -- IDLE Idle state + -- WAIT_FOR_DATA Wait for cancel service request + -- GET Get goal data from memory (Pointed by mem_data_c.addr) + -- GET_NEXT Get goal data from memory (Next goal from one pointed by mem_data_c.addr) + -- CHECK Determine what data has to be checked for each stored goal + -- CHECK_GOAL_ID Check if cancel goal id is same as mem_data_c + -- CHECK_STAMP Check if cancel goal stamp is earlier than the one in mem_data_c + -- CHECK_STATE Check if mem_data_c is in terminal state + -- WAIT_FOR_USER Wait for user input + -- CANCEL_GOAL Update goal state to CANCELING + -- ADD_CANCEL Store canceling goal for later response + -- SEND_RESPONSE Send cancel service response + -- WAIT_FOR_RET Wait for cancel service response + cancel_prc : process(all) + begin + -- DEFAULT + cancel_stage_next <= cancel_stage; + c_cnt_next <= c_cnt; + mem_data_c_next <= mem_data_c; + search_type_c_next <= search_type_c; + goals_canceling_cnt_next <= goals_canceling_cnt; + cancel_ret_code_next <= cancel_ret_code; + -- DEFAULT Unregistered + atomic_op_c <= '0'; + trigger_status_c <= '0'; + mem_start_c <= '0'; + mem_opcode_c <= NOP; + start_c <= '0'; + opcode_c <= NOP; + mem_c <= ZERO_GOAL_DATA; + cancel_request <= '0'; + cancel_request_handle <= GOAL_HANDLE_UNKNOWN; + goals_canceling_addr_c <= (others => '0'); + goals_canceling_ren_c <= '0'; + goals_canceling_wen_c <= '0'; + goals_canceling_ack_c <= '0'; + goals_canceling_goal_id_w_c <= (others => '0'); + goals_canceling_stamp_w_c <= (others => '0'); + + case (cancel_stage) is + when IDLE => + if (data_available_c = '1') then + start_c <= '1'; + opcode_c <= TAKE_REQUEST; + if (ack_c = '1') then + cancel_stage_next <= WAIT_FOR_DATA; + + --Reset + goals_canceling_cnt_next <= 0; + end if; + end if; + when WAIT_FOR_DATA => + if (done_c = '1') then + case (return_code_c) is + when ROS_RET_OK => + if (taken_c = '1') then + -- No Goals Available + if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then + cancel_ret_code_next <= CancelGoal_package.RR_ERROR_REJECTED; + cancel_stage_next <= SEND_RESPONSE; + else + if (to_UUID(goal_info_goal_id_c) = UUID_UNKNOWN) then + if (to_ROS_TIME(goal_info_stamp_c) = ROS_TIME_ZERO) then + search_type_c_next <= SEARCH_NONE; + else + search_type_c_next <= SEARCH_STAMP; + end if; + else + search_type_c_next <= SEARCH_GOAL_ID; + end if; + cancel_stage_next <= GET; + mem_data_c_next.addr <= mem_occupied_head; + end if; + else + cancel_stage_next <= IDLE; + end if; + when others => + -- TODO: Propagate Error? + cancel_stage_next <= IDLE; + end case; + end if; + when GET => + -- Atomic Operation Guard + if (not (atomic_op_r = '1' and mem_data_c.addr = mem_data_r.addr)) then + mem_start_c <= '1'; + mem_opcode_c <= GET; + mem_c.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG; + mem_c.addr <= mem_data_c.addr; + + if (mem_ack_c = '1') then + cancel_stage_next <= CHECK; + end if; + end if; + when GET_NEXT => + mem_start_c <= '1'; + mem_opcode_c <= GET_NEXT; + mem_c.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG; + mem_c.addr <= mem_data_c.addr; + + if (mem_ack_c = '1') then + cancel_stage_next <= CHECK; + end if; + when CHECK => + atomic_op_c <= '1'; + -- Wait for Memory + if (mem_done = '1') then + -- End of Memory + if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then + case (search_type_c) is + when SEARCH_GOAL_ID => + cancel_ret_code_next <= CancelGoal_package.RR_ERROR_UNKNOWN_GOAL_ID; + cancel_stage_next <= SEND_RESPONSE; + when others => + -- No Goals Canceled + if (goals_canceling_cnt = 0) then + cancel_ret_code_next <= CancelGoal_package.RR_ERROR_REJECTED; + cancel_stage_next <= SEND_RESPONSE; + else + cancel_ret_code_next <= CancelGoal_package.RR_ERROR_NONE; + cancel_stage_next <= SEND_RESPONSE; + end if; + end case; + else + assert check_mask(mem_data.field_flags, GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG) severity FAILURE; + + mem_data_c_next <= mem_data; + + -- NOTE: If both the Main and Cancel FSM enter the atomic operation zone at the smae time, the cancel FSM gives way and waits. + if (atomic_op_r = '1' and mem_data.addr = mem_data_r.addr) then + cancel_stage_next <= GET; + else + case (search_type_c) is + when SEARCH_GOAL_ID => + cancel_stage_next <= CHECK_GOAL_ID; + c_cnt_next <= 0; + when SEARCH_STAMP => + cancel_stage_next <= CHECK_STAMP; + when SEARCH_NONE => + cancel_stage_next <= CHECK_STATE; + end case; + end if; + end if; + end if; + when CHECK_GOAL_ID => + assert (not (atomic_op_r = '1' and mem_data_c.addr = mem_data_r.addr)) severity FAILURE; + atomic_op_c <= '1'; + + case (c_cnt) is + -- GOAL_ID 1/4 + when 0 => + if (mem_data_c.goal_id(0) = to_guid(goal_info_goal_id_c)(0)) then + c_cnt_next <= c_cnt + 1; + else + cancel_stage_next <= GET_NEXT; + end if; + -- GOAL_ID 2/4 + when 1 => + if (mem_data_c.goal_id(1) = to_guid(goal_info_goal_id_c)(1)) then + c_cnt_next <= c_cnt + 1; + else + cancel_stage_next <= GET_NEXT; + end if; + -- GOAL_ID 3/4 + when 2 => + if (mem_data_c.goal_id(2) = to_guid(goal_info_goal_id_c)(2)) then + c_cnt_next <= c_cnt + 1; + else + cancel_stage_next <= GET_NEXT; + end if; + -- GOAL_ID 4/4 + when 3 => + if (mem_data_c.goal_id(3) = to_guid(goal_info_goal_id_c)(3)) then + cancel_stage_next <= CHECK_STATE; + c_cnt_next <= 0; + else + cancel_stage_next <= GET_NEXT; + end if; + when others => + null; + end case; + when CHECK_STAMP => + assert (not (atomic_op_r = '1' and mem_data_c.addr = mem_data_r.addr)) severity FAILURE; + atomic_op_c <= '1'; + + if (mem_data_c.stamp < to_ROS_TIME(goal_info_stamp_c)) then + cancel_stage_next <= CHECK_STATE; + else + cancel_stage_next <= GET_NEXT; + end if; + when CHECK_STATE => + assert (not (atomic_op_r = '1' and mem_data_c.addr = mem_data_r.addr)) severity FAILURE; + atomic_op_c <= '1'; + + if (is_terminal(mem_data_c.state)) then + cancel_ret_code_next <= CancelGoal_package.RR_ERROR_GOAL_TERMINATED; + cancel_stage_next <= SEND_RESPONSE; + elsif (mem_data_c.state = GoalStatus_package.STATUS_CANCELING) then + -- Skip State Change + cancel_stage_next <= ADD_CANCEL; + else + cancel_stage_next <= WAIT_FOR_USER; + end if; + when WAIT_FOR_USER => + assert (not (atomic_op_r = '1' and mem_data_c.addr = mem_data_r.addr)) severity FAILURE; + atomic_op_c <= '1'; + + cancel_request <= '1'; + cancel_request_handle <= std_logic_vector(mem_data_c.addr); + if (cancel_response = '1') then + if (cancel_accepted = '1') then + cancel_stage_next <= CANCEL_GOAL; + else + case (search_type_c) is + when SEARCH_GOAL_ID => + if (to_ROS_TIME(goal_info_stamp_c) = ROS_TIME_ZERO) then + -- No Goals Canceled + cancel_ret_code_next <= CancelGoal_package.RR_ERROR_REJECTED; + cancel_stage_next <= SEND_RESPONSE; + else + -- Search for Timestamps + search_type_c_next <= SEARCH_STAMP; + cancel_stage_next <= GET; + mem_data_c_next.addr <= mem_occupied_head; + end if; + when SEARCH_STAMP => + -- NOTE: Since the goals are added chronologically, we don't have to check the timetsamps from now on + search_type_c_next <= SEARCH_NONE; + cancel_stage_next <= GET_NEXT; + when SEARCH_NONE => + -- Continue + cancel_stage_next <= GET_NEXT; + end case; + end if; + end if; + when CANCEL_GOAL => + assert (not (atomic_op_r = '1' and mem_data_c.addr = mem_data_r.addr)) severity FAILURE; + atomic_op_c <= '1'; + + mem_start_c <= '1'; + mem_opcode_c <= UPDATE; + mem_c.addr <= mem_data_c.addr; + mem_c.field_flags <= GMF_STATE_FLAG; + mem_c.state <= GoalStatus_package.STATUS_CANCELING; + + if (mem_ack_c = '1') then + cancel_stage_next <= ADD_CANCEL; + -- Trigger Status Update + trigger_status_c <= '1'; + end if; + when ADD_CANCEL => + goals_canceling_addr_c <= std_logic_vector(to_unsigned(goals_canceling_cnt,CancelGoal_package.RR_GOALS_CANCELING_GOAL_ID_ADDR_WIDTH)); + goals_canceling_wen_c <= '1'; + goals_canceling_goal_id_w_c <= std_logic_vector(to_unsigned(mem_data_c.goal_id)); + goals_canceling_stamp_w_c <= std_logic_vector(to_unsigned(mem_data_c.stamp)); + if (goals_canceling_ready_c = '1') then + -- Reached maximum number of cancel goals + if (goals_canceling_cnt = CancelGoal_package.RR_GOALS_CANCELING_MAX_DEPTH) then + -- DONE + cancel_ret_code_next <= CancelGoal_package.RR_ERROR_NONE; + cancel_stage_next <= SEND_RESPONSE; + else + goals_canceling_cnt_next <= goals_canceling_cnt + 1; + + case (search_type_c) is + when SEARCH_GOAL_ID => + if (to_ROS_TIME(goal_info_stamp_c) = ROS_TIME_ZERO) then + -- DONE + cancel_ret_code_next <= CancelGoal_package.RR_ERROR_NONE; + cancel_stage_next <= SEND_RESPONSE; + else + assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + -- Search for Timetamps + search_type_c_next <= SEARCH_STAMP; + cancel_stage_next <= GET; + mem_data_c_next.addr <= mem_occupied_head; + end if; + when SEARCH_STAMP => + -- NOTE: Since the goals are added chronologically, we don't have to check the timetsamps from now on + search_type_c_next <= SEARCH_NONE; + cancel_stage_next <= GET_NEXT; + when SEARCH_NONE => + -- Continue + cancel_stage_next <= GET_NEXT; + end case; + end if; + end if; + when SEND_RESPONSE => + start_c <= '1'; + opcode_c <= SEND_RESPONSE; + + if (ack_c = '1') then + cancel_stage_next <= WAIT_FOR_RET; + end if; + when WAIT_FOR_RET => + if (done_c = '1') then + -- TODO: Propagate Error? + cancel_stage_next <= IDLE; + end if; + when others => + null; + end case; + end process; + + -- *Status State Machine* + -- STATE DESCRIPTION + -- IDLE Idle state + -- GET_FIRST Get first goal data from memory + -- GET_NEXT Get goal data from memory (Next goal from one pointed by mem_data_c.addr) + -- WAIT_FOR_DATA Wait for memory data + -- PUSH_STATUS Add goal data to status list + -- PUBLISH Publish status list + -- WAIT_FOR_PUBLISHER Wait for status list publisher + status_prc : process (all) + begin + -- DEFAULT + status_stage_next <= status_stage; + trigger_status_next <= trigger_status; + status_list_cnt_next <= status_list_cnt; + mem_data_s_next <= mem_data_s; + -- DEFAULT Unregistered + status_idle_sig <= '0'; + start_s <= '0'; + opcode_s <= NOP; + mem_start_s <= '0'; + mem_opcode_s <= NOP; + mem_s <= ZERO_GOAL_DATA; + status_list_ren_s <= '0'; + status_list_wen_s <= '0'; + status_list_ack_s <= '0'; + status_list_addr_s <= (others => '0'); + status_list_goal_info_goal_id_w_s <= std_logic_vector(to_unsigned(UUID_UNKNOWN)); + status_list_goal_info_stamp_w_s <= std_logic_vector(to_unsigned(ROS_TIME_INVALID)); + status_list_status_w_s <= GoalStatus_package.STATUS_UNKNOWN; + + -- Trigger Status Setter + if (trigger_status_c = '1' or trigger_status_g = '1' or trigger_status_r = '1') then + trigger_status_next <= '1'; + end if; + + case (status_stage) is + when IDLE => + status_idle_sig <= '1'; + if (trigger_status = '1') then + -- Trigger Status Resetter + trigger_status_next <= '0'; + + -- Reset + status_list_cnt_next <= 0; + + if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then + status_stage_next <= PUBLISH; + else + status_stage_next <= GET_FIRST; + end if; + end if; + when GET_FIRST => + assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + mem_start_s <= '1'; + mem_opcode_s <= GET; + mem_s.addr <= mem_occupied_head; + mem_s.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG; + + if (mem_ack_s = '1') then + status_stage_next <= WAIT_FOR_DATA; + end if; + when GET_NEXT => + mem_start_s <= '1'; + mem_opcode_s <= GET_NEXT; + mem_s.addr <= mem_data_s.addr; + mem_s.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG; + + if (mem_ack_s = '1') then + status_stage_next <= WAIT_FOR_DATA; + end if; + when WAIT_FOR_DATA => + -- Wait for DATA + if (mem_done = '1') then + mem_data_s_next <= mem_data_s; + + -- No more Goals + if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then + status_stage_next <= PUBLISH; + else + status_stage_next <= PUSH_STATUS; + end if; + end if; + when PUSH_STATUS => + status_list_addr_s <= std_logic_vector(to_unsigned(status_list_cnt,GoalStatusArray_package.STATUS_LIST_GOAL_INFO_GOAL_ID_ADDR_WIDTH)); + status_list_wen_s <= '1'; + status_list_goal_info_goal_id_w_s <= std_logic_vector(to_unsigned(mem_data_s.goal_id)); + status_list_goal_info_stamp_w_s <= std_logic_vector(to_unsigned(mem_data_s.stamp)); + status_list_status_w_s <= mem_data_s.state; + + if (status_list_ready_s = '1') then + status_list_cnt_next <= status_list_cnt + 1; + + if (status_list_cnt = GoalStatusArray_package.STATUS_LIST_MAX_DEPTH-1) then + status_stage_next <= PUBLISH; + else + status_stage_next <= GET_NEXT; + end if; + end if; + when PUBLISH => + start_s <= '1'; + opcode_s <= PUBLISH; + + if (ack_s = '1') then + status_stage_next <= WAIT_FOR_PUBLISHER; + end if; + when WAIT_FOR_PUBLISHER => + if (done_s = '1') then + -- TODO: Propagate Error? + status_stage_next <= IDLE; + end if; + when others => + null; + end case; + end process; + + -- *Goal Memory State Machine* + -- STATE DESCRIPTION + -- IDLE Idle State. Done Signal is pulled high and Memory FSM accepts new memory operations + -- INSERT See Memory OPCODE Description + -- GET See Memory OPCODE Description + -- GET_NEXT See Memory OPCODE Description + -- GET_PREV See Memory OPCODE Description + -- UPDATE See Memory OPCODE Description + -- REMOVE See Memory OPCODE Description + -- RESET_MEMORY Reset goal memory to empty state + mem_ctrl_prc : process(all) + begin + -- DEFAULT + mem_stage_next <= mem_stage; + mem_cnt_next <= mem_cnt; + mem_occupied_head_next <= mem_occupied_head; + mem_occupied_tail_next <= mem_occupied_tail; + mem_empty_head_next <= mem_empty_head; + mem_empty_tail_next <= mem_empty_tail; + empty_head_res_ind_next <= empty_head_res_ind; + mem_latch_data_next <= mem_latch_data; + mem_data_next <= mem_data; + mem_addr_base_next <= mem_addr_base; + mem_addr_latch_next <= mem_addr_latch; + -- DEFAULT unregistered + mem_ack_g <= '0'; + mem_ack_r <= '0'; + mem_ack_c <= '0'; + mem_ack_s <= '0'; + mem_done <= '0'; + mem_abort_read <= '0'; + mem_ready_out <= '0'; + mem_valid_in <= '0'; + mem_read <= '0'; + mem_done <= '0'; + mem_addr <= (others => '0'); + mem_write_data <= (others => '0'); + + + case (mem_stage) is + -- NOTE: The REMOVE opcode sets mem_data.addr to the previous slot (or GOAL_MEMORY_MAX_ADDRESS if no previous goals) + -- This allows defined behavious of GET_NEXT when removing goals. + when IDLE => + mem_done <= '1'; + if (mem_start_g = '1') then + mem_ack_g <= '1'; + + case (mem_opcode_g) is + when INSERT => + assert (mem_empty_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; + + -- Latch DATA + mem_latch_data_next.goal_id <= to_guid(goal_id_g); + mem_latch_data_next.stamp <= g_stamp; + + mem_addr_base_next <= mem_empty_head; + mem_stage_next <= INSERT; + mem_cnt_next <= 0; + when others => + assert FALSE severity FAILURE; + end case; + elsif (mem_start_r = '1') then + mem_ack_r <= '1'; + + case (mem_opcode_r) is + when GET => + -- Latch DATA + mem_latch_data_next <= mem_r; + + if (mem_r.addr = mem_addr_base) then + mem_data_next.field_flags <= mem_data.field_flags or mem_r.field_flags; + else + mem_data_next <= ZERO_GOAL_DATA; + mem_data_next.field_flags <= mem_r.field_flags; + end if; + + mem_addr_base_next <= mem_r.addr; + mem_stage_next <= GET; + + if check_mask(mem_r.field_flags,GMF_STATE_FLAG) then + mem_cnt_next <= 0; + elsif check_mask(mem_r.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 1; + elsif check_mask(mem_r.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 5; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_r.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_r.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + -- DONE + mem_stage_next <= IDLE; + end if; + when GET_NEXT => + -- Latch DATA + mem_latch_data_next <= mem_r; + + mem_data_next <= ZERO_GOAL_DATA; + mem_data_next.field_flags <= mem_r.field_flags; + + mem_addr_base_next <= mem_r.addr; + mem_stage_next <= GET_NEXT; + mem_cnt_next <= 0; + when GET_PREV => + -- Latch DATA + mem_latch_data_next <= mem_r; + + mem_data_next <= ZERO_GOAL_DATA; + mem_data_next.field_flags <= mem_r.field_flags; + + mem_addr_base_next <= mem_r.addr; + mem_stage_next <= GET_PREV; + mem_cnt_next <= 0; + when UPDATE => + -- Latch DATA + mem_latch_data_next <= mem_r; + + if (mem_r.addr = mem_addr_base) then + mem_data_next.field_flags <= mem_data.field_flags or mem_r.field_flags; + else + mem_data_next <= ZERO_GOAL_DATA; + mem_data_next.field_flags <= mem_r.field_flags; + end if; + + mem_addr_base_next <= mem_r.addr; + mem_stage_next <= UPDATE; + + if check_mask(mem_r.field_flags,GMF_STATE_FLAG) then + mem_cnt_next <= 0; + elsif check_mask(mem_r.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 1; + elsif check_mask(mem_r.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 5; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_r.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_r.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + -- DONE + mem_stage_next <= IDLE; + end if; + when REMOVE => + mem_addr_base_next <= mem_r.addr; + mem_stage_next <= REMOVE; + mem_cnt_next <= 0; + when others => + assert FALSE severity FAILURE; + end case; + elsif (mem_start_c = '1') then + mem_ack_c <= '1'; + + case (mem_opcode_c) is + when GET => + -- Latch DATA + mem_latch_data_next <= mem_c; + + if (mem_c.addr = mem_addr_base) then + mem_data_next.field_flags <= mem_data.field_flags or mem_c.field_flags; + else + mem_data_next <= ZERO_GOAL_DATA; + mem_data_next.field_flags <= mem_c.field_flags; + end if; + + mem_addr_base_next <= mem_c.addr; + mem_stage_next <= GET; + + if check_mask(mem_c.field_flags,GMF_STATE_FLAG) then + mem_cnt_next <= 0; + elsif check_mask(mem_c.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 1; + elsif check_mask(mem_c.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 5; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_c.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_c.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + -- DONE + mem_stage_next <= IDLE; + end if; + when GET_NEXT => + -- Latch DATA + mem_latch_data_next <= mem_c; + + mem_data_next <= ZERO_GOAL_DATA; + mem_data_next.field_flags <= mem_c.field_flags; + + mem_addr_base_next <= mem_c.addr; + mem_stage_next <= GET_NEXT; + mem_cnt_next <= 0; + when GET_PREV => + -- Latch DATA + mem_latch_data_next <= mem_c; + + mem_data_next <= ZERO_GOAL_DATA; + mem_data_next.field_flags <= mem_c.field_flags; + + mem_addr_base_next <= mem_c.addr; + mem_stage_next <= GET_PREV; + mem_cnt_next <= 0; + when UPDATE => + -- Latch DATA + mem_latch_data_next <= mem_c; + + if (mem_c.addr = mem_addr_base) then + mem_data_next.field_flags <= mem_data.field_flags or mem_c.field_flags; + else + mem_data_next <= ZERO_GOAL_DATA; + mem_data_next.field_flags <= mem_c.field_flags; + end if; + + mem_addr_base_next <= mem_c.addr; + mem_stage_next <= UPDATE; + + if check_mask(mem_c.field_flags,GMF_STATE_FLAG) then + mem_cnt_next <= 0; + elsif check_mask(mem_c.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 1; + elsif check_mask(mem_c.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 5; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_c.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_c.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + -- DONE + mem_stage_next <= IDLE; + end if; + when others => + assert FALSE severity FAILURE; + end case; + elsif (mem_start_s = '1') then + mem_ack_s <= '1'; + + case (mem_opcode_s) is + when GET => + -- Latch DATA + mem_latch_data_next <= mem_s; + + if (mem_s.addr = mem_addr_base) then + mem_data_next.field_flags <= mem_data.field_flags or mem_s.field_flags; + else + mem_data_next <= ZERO_GOAL_DATA; + mem_data_next.field_flags <= mem_s.field_flags; + end if; + + mem_addr_base_next <= mem_s.addr; + mem_stage_next <= GET; + + if check_mask(mem_s.field_flags,GMF_STATE_FLAG) then + mem_cnt_next <= 0; + elsif check_mask(mem_s.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 1; + elsif check_mask(mem_s.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 5; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_s.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_s.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + -- DONE + mem_stage_next <= IDLE; + end if; + when GET_NEXT => + -- Latch DATA + mem_latch_data_next <= mem_s; + + mem_data_next <= ZERO_GOAL_DATA; + mem_data_next.field_flags <= mem_s.field_flags; + + mem_addr_base_next <= mem_s.addr; + mem_stage_next <= GET_NEXT; + mem_cnt_next <= 0; + when GET_PREV => + -- Latch DATA + mem_latch_data_next <= mem_s; + + mem_data_next <= ZERO_GOAL_DATA; + mem_data_next.field_flags <= mem_s.field_flags; + + mem_addr_base_next <= mem_s.addr; + mem_stage_next <= GET_PREV; + mem_cnt_next <= 0; + when others => + assert FALSE severity FAILURE; + end case; + end if; + when INSERT => + case (mem_cnt) is + -- GET Next Addr + when 0 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- READ Next Addr + when 1 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + -- SET New Empty Head + mem_empty_head_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); + + mem_cnt_next <= mem_cnt + 1; + end if; + -- GET Result Index (New Empty Head) + when 2 => + mem_valid_in <= '1'; + mem_addr <= mem_empty_head + GMF_RESULT_INDEX_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- READ Result Index (New Empty Head) + when 3 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + empty_head_res_ind_next <= unsigned(mem_read_data); + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Goal State + when 4 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STATE_OFFSET; + mem_write_data(CDR_INT8_WIDTH-1 downto 0) <= GoalStatus_package.STATUS_ACCEPTED; + mem_data_next.state <= GoalStatus_package.STATUS_ACCEPTED; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Goal ID 1/4 + when 5 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET; + mem_write_data <= mem_latch_data.goal_id(0); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Goal ID 2/4 + when 6 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 1; + mem_write_data <= mem_latch_data.goal_id(1); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Goal ID 3/4 + when 7 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 2; + mem_write_data <= mem_latch_data.goal_id(2); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Goal ID 4/4 + when 8 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 3; + mem_write_data <= mem_latch_data.goal_id(3); + mem_data_next.goal_id <= mem_latch_data.goal_id; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Stamp 1/2 + when 9 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STAMP_OFFSET; + mem_write_data <= mem_latch_data.stamp.sec; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Stamp 2/2 + when 10 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STAMP_OFFSET + 1; + mem_write_data <= mem_latch_data.stamp.nanosec; + mem_data_next.stamp <= mem_latch_data.stamp; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then + mem_cnt_next <= mem_cnt + 1; + else + mem_cnt_next <= mem_cnt + 3; + end if; + end if; + -- SET Deadline 1/2 + when 11 => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET; + mem_write_data <= ROS_TIME_INFINITE.sec; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + end if; + -- SET Deadline 2/2 + when 12 => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET + 1; + mem_write_data <= ROS_TIME_INFINITE.nanosec; + mem_data_next.deadline <= ROS_TIME_INFINITE; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + end if; + -- SET Prev Addr + when 13 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_PREV_ADDR_OFFSET; + mem_write_data <= std_logic_vector(resize(GOAL_MEMORY_MAX_ADDRESS,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Next Addr + when 14 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET; + mem_write_data <= std_logic_vector(resize(mem_occupied_head,WORD_WIDTH)); + mem_data_next.addr <= mem_addr_base; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + -- Update Occupied Head + mem_occupied_head_next <= mem_addr_base; + -- Initial Occupied Tail + if (mem_occupied_tail = GOAL_MEMORY_MAX_ADDRESS) then + mem_occupied_tail_next <= mem_addr_base; + end if; + + -- DONE + mem_stage_next <= IDLE; + end if; + when others => + null; + end case; + when GET => + case (mem_cnt) is + -- GET Goal State + when 0 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STATE_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 1; + elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 5; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + mem_cnt_next <= 10; + end if; + end if; + -- GET Goal ID 1/4 + when 1 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- GET Goal ID 2/4 + when 2 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 1; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- GET Goal ID 3/4 + when 3 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 2; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- GET Goal ID 4/4 + when 4 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 3; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 5; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then + mem_cnt_next <= 10; + else + mem_cnt_next <= 11; + end if; + end if; + end if; + -- GET Stamp 1/2 + when 5 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STAMP_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- GET Stamp 2/2 + when 6 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STAMP_OFFSET + 1; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then + mem_cnt_next <= 10; + elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 11; + else + mem_cnt_next <= 15; + end if; + end if; + end if; + -- GET Deadline 1/2 + when 7 => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + end if; + -- GET Deadline 2/2 + when 8 => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET + 1; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then + mem_cnt_next <= 10; + elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 11; + elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 15; + else + mem_cnt_next <= 17; + end if; + end if; + end if; + end if; + -- GET Result Index + when 9 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_RESULT_INDEX_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then + mem_cnt_next <= 10; + elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 11; + elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 15; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 17; + else + mem_cnt_next <= 19; + end if; + end if; + -- READ Goal State + when 10 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_data_next.state <= mem_read_data(CDR_INT8_WIDTH-1 downto 0); + + if check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 11; + elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 15; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 17; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 19; + else + mem_stage_next <= IDLE; + end if; + end if; + -- READ Goal ID 1/4 + when 11 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_data_next.goal_id(0) <= mem_read_data; + + mem_cnt_next <= mem_cnt + 1; + end if; + -- READ Goal ID 2/4 + when 12 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_data_next.goal_id(1) <= mem_read_data; + + mem_cnt_next <= mem_cnt + 1; + end if; + -- READ Goal ID 3/4 + when 13 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_data_next.goal_id(2) <= mem_read_data; + + mem_cnt_next <= mem_cnt + 1; + end if; + -- READ Goal ID 4/4 + when 14 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_data_next.goal_id(3) <= mem_read_data; + + if check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 15; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 17; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 19; + else + mem_stage_next <= IDLE; + end if; + end if; + -- READ Stamp 1/2 + when 15 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_data_next.stamp.sec <= mem_read_data; + + mem_cnt_next <= mem_cnt + 1; + end if; + -- READ Stamp 2/2 + when 16 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_data_next.stamp.nanosec <= mem_read_data; + + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 17; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 19; + else + mem_stage_next <= IDLE; + end if; + end if; + -- READ Deadline 1/2 + when 17 => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_data_next.deadline.sec <= mem_read_data; + + mem_cnt_next <= mem_cnt + 1; + end if; + end if; + -- READ Deadline 2/2 + when 18 => + -- Synthesis Guard + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_data_next.deadline.nanosec <= mem_read_data; + + if check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 19; + else + mem_stage_next <= IDLE; + end if; + end if; + end if; + -- READ Result index + when 19 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_data_next.res_ind <= mem_read_data; + + mem_stage_next <= IDLE; + end if; + when others => + null; + end case; + when GET_NEXT => + case (mem_cnt) is + -- GET Next Addr + when 0 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- READ Next Addr + when 1 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_addr_base_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); + mem_stage_next <= GET; + + if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then + mem_cnt_next <= 0; + elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 1; + elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 5; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + -- DONE + mem_stage_next <= IDLE; + end if; + end if; + when others => + null; + end case; + when GET_PREV => + case (mem_cnt) is + -- GET Previous Addr + when 0 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_PREV_ADDR_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- READ Previous Addr + when 1 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_addr_base_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); + mem_stage_next <= GET; + + if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then + mem_cnt_next <= 0; + elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 1; + elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 5; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + -- DONE + mem_stage_next <= IDLE; + end if; + end if; + when others => + null; + end case; + when UPDATE => + case (mem_cnt) is + -- SET Goal State + when 0 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STATE_OFFSET; + mem_write_data <= (others => '0'); + mem_write_data(CDR_INT8_WIDTH-1 downto 0) <= mem_latch_data.state; + mem_data_next.state <= mem_latch_data.state; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then + mem_cnt_next <= 1; + elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 5; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + mem_stage_next <= IDLE; + end if; + end if; + -- SET Goal ID 1/4 + when 1 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET; + mem_write_data <= mem_latch_data.goal_id(0); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Goal ID 2/4 + when 2 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 1; + mem_write_data <= mem_latch_data.goal_id(1); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Goal ID 3/4 + when 3 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 2; + mem_write_data <= mem_latch_data.goal_id(3); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Goal ID 4/4 + when 4 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 3; + mem_write_data <= mem_latch_data.goal_id(3); + mem_data_next.goal_id <= mem_latch_data.goal_id; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then + mem_cnt_next <= 5; + elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + mem_stage_next <= IDLE; + end if; + end if; + -- SET Stamp 1/2 + when 5 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STAMP_OFFSET; + mem_write_data <= mem_latch_data.stamp.sec; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Stamp 2/2 + when 6 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_STAMP_OFFSET + 1; + mem_write_data <= mem_latch_data.stamp.nanosec; + mem_data_next.stamp <= mem_latch_data.stamp; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then + mem_cnt_next <= 7; + elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + mem_stage_next <= IDLE; + end if; + end if; + -- SET Deadline 1/2 + when 7 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET; + mem_write_data <= mem_latch_data.deadline.sec; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Deadline 2/2 + when 8 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET + 1; + mem_write_data <= mem_latch_data.deadline.sec; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then + mem_cnt_next <= 9; + else + mem_stage_next <= IDLE; + end if; + end if; + -- SET Result Index + when 9 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_RESULT_INDEX_OFFSET; + mem_write_data <= mem_latch_data.res_ind; + mem_data_next.res_ind <= mem_latch_data.res_ind; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + -- DONE + mem_stage_next <= IDLE; + end if; + when others => + null; + end case; + when REMOVE => + case (mem_cnt) is + -- GET Next Addr + when 0 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- GET Previous Addr + when 1 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_PREV_ADDR_OFFSET; + mem_read <= '1'; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Next Addr + when 2 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET; + mem_write_data <= std_logic_vector(resize(GOAL_MEMORY_MAX_ADDRESS,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + -- Set New Empty Head + mem_empty_head_next <= mem_addr_base; + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Next Addr (Current Tail) + when 3 => + mem_valid_in <= '1'; + mem_addr <= mem_empty_tail + GMF_NEXT_ADDR_OFFSET; + mem_write_data <= std_logic_vector(resize(mem_addr_base,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + -- Set New Empty TAIL + mem_empty_tail_next <= mem_addr_base; + mem_cnt_next <= mem_cnt + 1; + end if; + -- READ Next Addr + when 4 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + mem_addr_latch_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); + mem_cnt_next <= mem_cnt + 1; + end if; + -- READ Previous Addr + when 5 => + mem_ready_out <= '1'; + + -- Memory Flow Control Guard + if (mem_valid_out = '1') then + -- Set mem_data.addr to previous slot + mem_data_next.addr <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); + + if (mem_addr_latch = GOAL_MEMORY_MAX_ADDRESS) then + -- Set New Occupied Tail + mem_occupied_tail_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); + + if (resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH) = GOAL_MEMORY_MAX_ADDRESS) then + -- Reset Occupied Head + mem_occupied_head_next <= GOAL_MEMORY_MAX_ADDRESS; + mem_stage_next <= IDLE; + else + mem_addr_base_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); + mem_cnt_next <= mem_cnt + 2; + end if; + else + mem_addr_base_next <= mem_addr_latch; + mem_addr_latch_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); + mem_cnt_next <= mem_cnt + 1; + end if; + end if; + -- SET Previous Addr (Next Slot) + when 6 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_PREV_ADDR_OFFSET; + mem_write_data <= std_logic_vector(resize(mem_addr_latch,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if (mem_addr_latch = GOAL_MEMORY_MAX_ADDRESS) then + -- Set New Occupied Head + mem_occupied_head_next <= mem_addr_base; + -- DONE + mem_stage_next <= IDLE; + else + mem_addr_base_next <= mem_addr_latch; + mem_addr_latch_next <= mem_addr_base; + mem_cnt_next <= mem_cnt + 1; + end if; + end if; + -- SET Next Addr (Previous Slot) + when 7 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET; + mem_write_data <= std_logic_vector(resize(mem_addr_latch,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + -- DONE + mem_stage_next <= IDLE; + end if; + when others => + null; + end case; + when RESET_MEMORY => + case (mem_cnt) is + -- SET Result Index + when 0 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_RESULT_INDEX_OFFSET; + mem_write_data <= std_logic_vector(empty_head_res_ind); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Next Pointer + when 1 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET; + if (mem_addr_base = MAX_GOAL_ADDRESS) then + mem_write_data <= std_logic_vector(resize(GOAL_MEMORY_MAX_ADDRESS,WORD_WIDTH)); + else + mem_write_data <= std_logic_vector(resize(mem_addr_base + GOAL_FRAME_SIZE,WORD_WIDTH)); + end if; + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + mem_cnt_next <= mem_cnt + 1; + end if; + -- SET Previous Pointer + when 2 => + mem_valid_in <= '1'; + mem_addr <= mem_addr_base + GMF_PREV_ADDR_OFFSET; + mem_write_data <= std_logic_vector(resize(mem_addr_latch,WORD_WIDTH)); + + -- Memory Flow Control Guard + if (mem_ready_in = '1') then + if (mem_addr_base = MAX_GOAL_ADDRESS) then + -- Initialize Empty and Occupied Heads + mem_empty_head_next <= FIRST_GOAL_ADDRESS; + mem_empty_tail_next <= MAX_GOAL_ADDRESS; + mem_occupied_head_next <= GOAL_MEMORY_MAX_ADDRESS; + mem_occupied_tail_next <= GOAL_MEMORY_MAX_ADDRESS; + empty_head_res_ind_next <= (others => '0'); + + -- DONE + mem_stage_next <= IDLE; + else + empty_head_res_ind_next <= empty_head_res_ind + 1; + mem_addr_latch_next <= mem_addr_base; + mem_addr_base_next <= mem_addr_base + GOAL_FRAME_SIZE; + mem_cnt_next <= 0; + end if; + end if; + when others => + null; + end case; + end case; + end process; + + sync_prc : process(clk) + begin + if rising_edge(clk) then + if (reset = '1') then + stage <= RESET_MEMORY; + goal_stage <= IDLE; + cancel_stage <= IDLE; + status_stage <= IDLE; + mem_stage <= RESET_MEMORY; + mem_data_r <= ZERO_GOAL_DATA; + mem_data_c <= ZERO_GOAL_DATA; + mem_data_s <= ZERO_GOAL_DATA; + mem_latch_data <= ZERO_GOAL_DATA; + mem_data <= ZERO_GOAL_DATA; + rrq_addr_base <= FIRST_RRQ_ADDRESS; + rrq_addr_latch <= RRQ_MEMORY_MAX_ADDRESS; + mem_addr_base <= FIRST_GOAL_ADDRESS; + mem_addr_latch <= GOAL_MEMORY_MAX_ADDRESS; + rrq_empty_head <= RRQ_MEMORY_MAX_ADDRESS; + rrq_occupied_head <= RRQ_MEMORY_MAX_ADDRESS; + mem_occupied_head <= GOAL_MEMORY_MAX_ADDRESS; + mem_occupied_tail <= GOAL_MEMORY_MAX_ADDRESS; + mem_empty_head <= GOAL_MEMORY_MAX_ADDRESS; + mem_empty_tail <= GOAL_MEMORY_MAX_ADDRESS; + return_code_latch <= ROS_RET_OK; + request_id_latch <= EMPTY_REQUEST_ID; + check_time <= ROS_TIME_INFINITE; + time_latch <= ROS_TIME_INVALID; + goal_handle_latch <= GOAL_HANDLE_UNKNOWN; + goal_state_latch <= GoalStatus_package.STATUS_UNKNOWN; + g_stamp <= ROS_TIME_INVALID; + search_type_c <= SEARCH_NONE; + cancel_ret_code <= CancelGoal_package.RR_ERROR_NONE; + cnt <= 0; + terminal_cnt <= 0; + c_cnt <= 0; + goals_canceling_cnt <= 0; + status_list_cnt <= 0; + mem_cnt <= 0; + trigger_result <= '0'; + trigger_status <= '1'; -- Trigger Status Update immediately + g_accept <= '0'; + empty_head_res_ind <= (others => '0'); + else + stage <= stage_next; + goal_stage <= goal_stage_next; + cancel_stage <= cancel_stage_next; + status_stage <= status_stage_next; + mem_stage <= mem_stage_next; + mem_data_r <= mem_data_r_next; + mem_data_c <= mem_data_c_next; + mem_data_s <= mem_data_s_next; + mem_latch_data <= mem_latch_data_next; + mem_data <= mem_data_next; + rrq_addr_base <= rrq_addr_base_next; + rrq_addr_latch <= rrq_addr_latch_next; + mem_addr_base <= mem_addr_base_next; + mem_addr_latch <= mem_addr_latch_next; + rrq_empty_head <= rrq_empty_head_next; + rrq_occupied_head <= rrq_occupied_head_next; + mem_occupied_head <= mem_occupied_head_next; + mem_occupied_tail <= mem_occupied_tail_next; + mem_empty_head <= mem_empty_head_next; + mem_empty_tail <= mem_empty_tail_next; + return_code_latch <= return_code_latch_next; + request_id_latch <= request_id_latch_next; + check_time <= check_time_next; + time_latch <= time_latch_next; + goal_handle_latch <= goal_handle_latch_next; + goal_state_latch <= goal_state_latch_next; + g_stamp <= g_stamp_next; + search_type_c <= search_type_c_next; + cancel_ret_code <= cancel_ret_code_next; + cnt <= cnt_next; + terminal_cnt <= terminal_cnt_next; + c_cnt <= c_cnt_next; + goals_canceling_cnt <= goals_canceling_cnt_next; + status_list_cnt <= status_list_cnt_next; + mem_cnt <= mem_cnt_next; + trigger_result <= trigger_result_next; + trigger_status <= trigger_status_next; + g_accept <= g_accept_next; + empty_head_res_ind <= empty_head_res_ind_next; + end if; + end if; + end process; + +end architecture; diff --git a/src/ros2/ros_package.vhd b/src/ros2/ros_package.vhd index 9b8001d..997dc14 100644 --- a/src/ros2/ros_package.vhd +++ b/src/ros2/ros_package.vhd @@ -25,13 +25,23 @@ package ros_package is nanosec : std_logic_vector(CDR_LONG_WIDTH-1 downto 0); end record; + constant ROS_TIME_ZERO : ROS_TIME_TYPE := (sec => (others => '0'), nanosec => (others => '0')); + constant ROS_TIME_INFINITE : ROS_TIME_TYPE := (sec => (others => '1'), nanosec => (others => '1')); + constant ROS_TIME_INVALID : ROS_TIME_TYPE := (sec => (others => '1'), nanosec => x"fffffffe"); + subtype ROS_DURATION_TYPE is ROS_TIME_TYPE; + constant ROS_DURATION_ZERO : ROS_DURATION_TYPE := (sec => (others => '0'), nanosec => (others => '0')); + constant ROS_DURATION_INFINITE : ROS_DURATION_TYPE := (sec => (others => '1'), nanosec => (others => '1')); + constant ROS_TIME_WIDTH : natural := 64; function to_unsigned(input : ROS_TIME_TYPE) return unsigned; function to_ROS_TIME(input : std_logic_vector) return ROS_TIME_TYPE; + -- Helper Function + function gen_duration(s,ns : integer) return ROS_DURATION_TYPE; + function gen_duration(t : time) return ROS_DURATION_TYPE; type ROS_QOS_PROFILE_TYPE is record HISTORY_QOS : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0); @@ -263,7 +273,7 @@ package ros_package is type ROS_TOPIC_OPCODE_TYPE is (NOP, PUBLISH, TAKE); type ROS_SERVICE_OPCODE_TYPE is (NOP, SEND_REQUEST, TAKE_REQUEST, SEND_RESPONSE, TAKE_RESPONSE); - type ROS_ACTION_OPCODE_TYPE is (NOP, SEND_GOAL_REQUEST, TAKE_GOAL_REQUEST, SEND_GOAL_RESPONSE, TAKE_GOAL_RESPONSE, SEND_RESULT_REQUEST, TAKE_RESULT_REQUEST, SEND_RESULT_RESPONSE, TAKE_RESULT_RESPONSE, SEND_CANCEL_REQUEST, TAKE_CANCEL_REQUEST, SEND_CANCEL_RESPONSE, TAKE_CANCEL_RESPONSE, PUBLISH_FEEDBACK, ACCEPT_GOAL, UPDATE_GOAL, EXPIRE_GOAL); + type ROS_ACTION_OPCODE_TYPE is (NOP, GET_GOAL, GET_LAST_GOAL, GET_PREVIOUS_GOAL, UPDATE_GOAL_STATE, PUBLISH_FEEDBACK); constant ROS_SEQUENCE_ID_WIDTH : natural := 64; @@ -271,6 +281,9 @@ package ros_package is constant SERVICE_OVERHEAD_BYTES : natural := 16; + constant GOAL_HANDLE_WIDTH : natural := WORD_WIDTH; + constant GOAL_HANDLE_UNKNOWN : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0) := (others => '1'); + function get_num_pubs(nodes : ROS_NODE_ARRAY_TYPE) return natural; function get_num_subs(nodes : ROS_NODE_ARRAY_TYPE) return natural; function get_num_services(nodes : ROS_NODE_ARRAY_TYPE) return natural; @@ -323,10 +336,41 @@ package ros_package is type SERVICE_INTERFACE_ARRAY_TYPE is array (natural range <>) of SERVICE_INTERFACE_TYPE; + function ">" (L,R: ROS_TIME_TYPE) return boolean; + function "<" (L,R: ROS_TIME_TYPE) return boolean; + function ">=" (L,R: ROS_TIME_TYPE) return boolean; + function "<=" (L,R: ROS_TIME_TYPE) return boolean; + function "=" (L,R: ROS_TIME_TYPE) return boolean; + function "/=" (L,R: ROS_TIME_TYPE) return boolean; + end package; package body ros_package is + function gen_duration(s,ns : integer) return ROS_DURATION_TYPE is + variable ret : ROS_DURATION_TYPE := ROS_TIME_ZERO; + constant sec : natural := 10**9; + begin + assert (ns < sec) report "ns argument has to be less than a second" severity failure; + + ret.sec := std_logic_vector(to_unsigned(s,ret.sec'length)); + ret.nanosec := std_logic_vector(to_unsigned(ns,ret.nanosec'length)); + + return ret; + end function; + + function gen_duration(t : time) return ROS_DURATION_TYPE is + variable seconds : natural; + variable nanoseconds : natural; + begin + -- Extract Seconds + seconds := t / sec; + -- Extract Nanoseconds + nanoseconds := (t - (seconds * sec)) / ns; + + return gen_duration(seconds, nanoseconds); + end function; + function to_gid(guid : GUID_TYPE) return GID_TYPE is variable ret : GID_TYPE; begin @@ -599,4 +643,34 @@ package body ros_package is return ret; end function; + function ">" (L,R: ROS_TIME_TYPE) return boolean is + begin + return to_unsigned(L) > to_unsigned(R); + end function; + + function "<" (L,R: ROS_TIME_TYPE) return boolean is + begin + return to_unsigned(L) < to_unsigned(R); + end function; + + function ">=" (L,R: ROS_TIME_TYPE) return boolean is + begin + return to_unsigned(L) >= to_unsigned(R); + end function; + + function "<=" (L,R: ROS_TIME_TYPE) return boolean is + begin + return to_unsigned(L) <= to_unsigned(R); + end function; + + function "=" (L,R: ROS_TIME_TYPE) return boolean is + begin + return to_unsigned(L) = to_unsigned(R); + end function; + + function "/=" (L,R: ROS_TIME_TYPE) return boolean is + begin + return to_unsigned(L) /= to_unsigned(R); + end function; + end package body; diff --git a/src/rtps_package.vhd b/src/rtps_package.vhd index cd08ecf..db2b145 100644 --- a/src/rtps_package.vhd +++ b/src/rtps_package.vhd @@ -574,6 +574,7 @@ package rtps_package is end record; function to_guid(A : GUIDPREFIX_TYPE; B : std_logic_vector(ENTITYID_WIDTH-1 downto 0)) return GUID_TYPE; + function to_guid(A : std_logic_vector) return GUID_TYPE; function to_key_hash(A : GUID_TYPE) return KEY_HASH_TYPE; function to_key_hash(A : std_logic_vector) return KEY_HASH_TYPE; @@ -634,7 +635,7 @@ package body rtps_package is constant sec : natural := 10**9; constant half_sec : natural := sec/2; begin - assert (ns < 10**9) report "ns argument has to be less than a second" severity failure; + assert (ns < sec) report "ns argument has to be less than a second" severity failure; ret(0) := to_unsigned(s, WORD_WIDTH); -- If Fraction Bit is >= 500 ms it cannot be represented as a natural (because naturals/integers are signed). @@ -652,7 +653,6 @@ package body rtps_package is function gen_duration(t : time) return DURATION_TYPE is variable seconds : natural; variable nanoseconds : natural; - variable tmp : time; begin -- Extract Seconds seconds := t / sec; @@ -1044,16 +1044,19 @@ package body rtps_package is function get_sub_vector (input : std_logic_vector; index : natural; width : natural; invert : boolean) return std_logic_vector is variable ret : std_logic_vector(width-1 downto 0) := (others => '0'); + variable input2 : std_logic_vector(input'length-1 downto 0) := (others => '0'); begin assert(input'length mod width = 0) report "Input Length has to be multiple of width" severity FAILURE; assert(input'length / width > index) report "Index out of bounds" severity FAILURE; + input2 := input; -- Force input to "downto" range + if (invert = TRUE) then -- XXX: Synthesis Hack --ret := input(input'length-(index*width)-1 downto input'length-((index+1)*width)); for i in 0 to (input'length/width)-1 loop if (index = i) then - ret := input(input'length-(i*width)-1 downto input'length-((i+1)*width)); + ret := input2(input'length-(i*width)-1 downto input'length-((i+1)*width)); end if; end loop; else @@ -1061,7 +1064,7 @@ package body rtps_package is --ret := input(((index+1)*width)-1 downto index*width); for i in 0 to (input'length/width)-1 loop if (index = i) then - ret := input(((i+1)*width)-1 downto i*width); + ret := input2(((i+1)*width)-1 downto i*width); end if; end loop; end if; @@ -1097,4 +1100,15 @@ package body rtps_package is return ret; end function; + + function to_guid(A : std_logic_vector) return GUID_TYPE is + variable ret : GUID_TYPE; + begin + assert (A'length = GUID_WIDTH) report "SLV Length missmatch" severity FAILURE; + ret := GUID_UNKNOWN; + for i in 0 to ret'length-1 loop + ret(i) := get_sub_vector(A, i, ret(i)'length, TRUE); + end loop; + return ret; + end function; end package body;