-- 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 candidates (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 deadline 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 status 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; -- Acknowledge memory request 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 frame 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 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_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 same 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 timestamps 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 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 timestamps 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 behaviours 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;