-- 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; -- ROS ACTION SERVER -- This Entity is implementing the ROS Action Server functionality, and keeps user interaction to a minimum. -- This server does not contain any type-dependent code, and is therefore static for all ROS actions. A wrapper -- has to present the type dependent ports of the underlying ROS entities to the user next to the signals of this -- entity. This wrapper entity should also contain a "memory array" (equal in depth to the MAX_GOALS generic) of -- results, that the user has direct access to. -- Internally the entity contains 2 memories, one for accepted goals tracking their status (size configurable via the -- MAX_GOALS generic), and one for stored result requests in order to reply as soon as the respective goal is finished -- (size configurable via the MAX_RESULT_REQUESTS generic). -- This entity uses the memory addresses of the stored goals in memory as a handles with which the user can reference -- goals for operations. -- Whenever the GOAL service has available data, the goal request is fetched and presented to the user -- ('new_goal_request' signal asserted, 'new_goal_handle' signal containing the goal handle of the new goal, and -- 'new_goal_result_index' signal containing the target memory address/index of the result array for this goal). -- The entity waits until the user asserts the 'new_goal_response' signal, and the 'new_goal_accepted' signal denotes -- if the user accepted (signal asserted) or rejected the goal request, and the GOAL Service response is sent -- accordingly. -- If the goal memory is full and the TIMEOUT_DURATION generic is set to infinite duration, the entity will remove the -- oldest stored goal in a terminal state, if a new goal request is available from the GOAL service. If on the other -- hand the the TIMEOUT_DURATION is set, the goal will be automatically removed by the configured duration after the -- goal enters a terminal state. -- Whenever the RESULT service has available data, the result request is fetched and handled. If the target goal is -- not stored in the memory or the goal has already entered a terminal state, the response is sent immediately. -- If the target goal has not yet entered a terminal state, the result request is stored in the result request memory. -- (New result requests from the RESULT service are only fetched if the result memory has space). -- Whenever a goal enters a terminal state, the stored result requests of the respective goal are handled and removed -- from the memory. -- Whenever the CANCEL service has available data, the cancel request is fetched, and all relevant goal cancellation -- requests are presented to the user ('cancel_request' signal asserted, and 'cancel_request_handle' signal contains -- the goal handle of the goal to be canceled). The entity waits until the user asserts the 'cancel_response' signal, -- and the 'cancel_accepted' signal denotes if the user accepted (signal asserted) or rejected the cancellation request, -- and the CANCEL service response is sent accordingly. -- The allowed ROS action server operations are GET_GOAL, GET_LAST_GOAL, GET_FIRST_GOAL, GET_NEXT_GOAL, -- GET_PREVIOUS_GOAL, UPDATE_GOAL_STATE, PUBLISH_FEEDBACK. -- To start an operation the 'start' signal is asserted with the respective opcode in the 'opcode' signal until the -- operation is acknowledged by the entity by asserting the 'ack' signal. After the operation is completed, the 'done' -- signal is asserted for one clock cycle, and the return code can be found in the 'return_code' signal on the same -- clock cycle. -- The GET_GOAL operation allows the user to get the goal ID, the current goal state, the target memory address/index -- of the result array, the goal acceptance timestamp, and the goal handle of a particular goal denoted by the goal -- handle in the 'goal_handle_in' in the 'goal_id', 'goal_state_out', 'goal_result_index', 'goal_stamp', and -- 'goal_handle_out' signals, respectively. -- Similarly the GET_FIRST_GOAL and GET_LAST_GOAL operations return the oldest and newest accepted goal in the list, -- whereas the GET_NEXT_GOAL and GET_PREVIOUS_GOAL operations return the next and previous goal based on the goal handle -- provided in the 'goal_handle_in' signal, and can be used to iterate through the goal list. -- The UPDATE_GOAL_STATE operation is used to update the goal state of the goal denoted by the goal handle in the -- 'goal_handle_in' signal to the state provided in the 'goal_state_in' signal. When transitioning to the "SUCCEEDED" -- state the user has to make sure, that the goal results are stored in the respective memory address/index of result -- array prior to this operation. -- The PUBLISH_FEEDBACK operation triggers the server to publish in the feedback topic and is used to publish feedback -- for the goal denoted by the goal handle in the 'goal_handle_in' signal. The user has to hold the type-dependent -- feedback signals valid until the return of the operation. -- NOTE: New Goals coming from the Goal Service are not checked against their goal id, which may lead to multiple -- goals with same goal id being stored in memory. But this is expected behaviour and not the responsibility -- of the action server. -- NOTE: If both the goal id and time of a cancel request are set, and the goal with the set goal id falls also in -- the requested cancellation time range, a second user cancel prompt on the same goal may occur if the first -- prompt is rejected. -- MEMORY LAYOUT -- This entity is using double linked lists of fixed-size memory frames. -- For each memory 2 lists of frames exist, one for the empty frames, and one for the occupied frames. -- The memory frames have following structure: -- GOAL DATA MEMORY FORMAT -- ======================= -- 31............24..............16..............8...............0 -- | | | | | -- +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ -- +-----------------------------------------------+---------------+ -- 01| UNUSED | GOAL_STATE | -- +-----------------------------------------------+---------------+ -- 02| | -- + + -- 03| | -- + GOAL_ID + -- 04| | -- + + -- 05| | -- +---------------------------------------------------------------+ -- 06| | -- + STAMP + -- 07| | -- +---------------------------------------------------------------+ -- 08| | -- + DEADLINE + -- 09| [only if TIMEOUT_DURATION /= DURATION_INFINITE] | -- +---------------------------------------------------------------+ -- 10| RESULT_INDEX | -- +---------------------------------------------------------------+ -- 11| NEXT_ADDRESS | -- +---------------------------------------------------------------+ -- 12| PREV_ADDRESS | -- +---------------------------------------------------------------+ -- RESULT REQUEST DATA MEMORY FORMAT -- ================================= -- 31............24..............16..............8...............0 -- | | | | | -- +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ -- +---------------------------------------------------------------+ -- 01| GOAL_HANDLE | -- +---------------------------------------------------------------+ -- 02| | -- + + -- 03| | -- + + -- 04| | -- + REQUEST_ID + -- 05| | -- + + -- 06| | -- + + -- 07| | -- +---------------------------------------------------------------+ -- 08| NEXT_ADDRESS | -- +---------------------------------------------------------------+ -- 09| PREV_ADDRESS | -- +---------------------------------------------------------------+ entity ros_action_server is generic ( TIMEOUT_DURATION : ROS_DURATION_TYPE; MAX_GOALS : natural; MAX_RESULT_REQUESTS : natural; ENABLE_FEEDBACK : std_logic := '1' ); port ( -- SYSTEM clk : in std_logic; reset : in std_logic; time : in ROS_TIME_TYPE; -- *GOAL SERVICE* start_g : out std_logic; ack_g : in std_logic; opcode_g : out ROS_SERVICE_OPCODE_TYPE; service_info_g : in SERVICE_INFO_TYPE; request_id_g : out REQUEST_ID_TYPE; data_available_g : in std_logic; taken_g : in std_logic; done_g : in std_logic; return_code_g : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); -- REQUEST goal_id_g : in std_logic_vector(UUID_WIDTH-1 downto 0); -- RESPONSE accepted_g : out std_logic; stamp_g : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0); -- *RESULT SERVICE* start_r : out std_logic; ack_r : in std_logic; opcode_r : out ROS_SERVICE_OPCODE_TYPE; service_info_r : in SERVICE_INFO_TYPE; request_id_r : out REQUEST_ID_TYPE; data_available_r : in std_logic; taken_r : in std_logic; done_r : in std_logic; return_code_r : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); result_index : out std_logic_vector(WORD_WIDTH-1 downto 0); result_sel : out std_logic; result_sel_ack : in std_logic; -- REQUEST goal_id_r : in std_logic_vector(UUID_WIDTH-1 downto 0); -- RESPONSE status_r : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0); -- *CANCEL SERVICE* start_c : out std_logic; ack_c : in std_logic; opcode_c : out ROS_SERVICE_OPCODE_TYPE; service_info_c : in SERVICE_INFO_TYPE; request_id_c : out REQUEST_ID_TYPE; data_available_c : in std_logic; taken_c : in std_logic; done_c : in std_logic; return_code_c : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); -- REQUEST goal_info_goal_id_c : in std_logic_vector(UUID_WIDTH-1 downto 0); goal_info_stamp_c : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0); -- RESPONSE cancel_return_code_c : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0); goals_canceling_len_c : out std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0); goals_canceling_addr_c : out std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0); goals_canceling_ready_c : in std_logic; goals_canceling_ren_c : out std_logic; goals_canceling_wen_c : out std_logic; goals_canceling_valid_c : in std_logic; goals_canceling_ack_c : out std_logic; goals_canceling_goal_id_r_c : in std_logic_vector(UUID_WIDTH-1 downto 0); goals_canceling_goal_id_w_c : out std_logic_vector(UUID_WIDTH-1 downto 0); goals_canceling_stamp_r_c : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0); goals_canceling_stamp_w_c : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0); -- *FEEDBACK TOPIC* start_fb : out std_logic; opcode_fb : out ROS_TOPIC_OPCODE_TYPE; ack_fb : in std_logic; done_fb : in std_logic; return_code_fb : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); goal_id_fb : out std_logic_vector(UUID_WIDTH-1 downto 0); -- *STATUS TOPIC* start_s : out std_logic; opcode_s : out ROS_TOPIC_OPCODE_TYPE; ack_s : in std_logic; done_s : in std_logic; return_code_s : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); status_list_len_s : out std_logic_vector(GoalStatusArray_package.STATUS_LIST_ADDR_WIDTH-1 downto 0); status_list_addr_s : out std_logic_vector(GoalStatusArray_package.STATUS_LIST_ADDR_WIDTH-1 downto 0); status_list_ready_s : in std_logic; status_list_ren_s : out std_logic; status_list_wen_s : out std_logic; status_list_valid_s : in std_logic; status_list_ack_s : out std_logic; status_list_goal_info_goal_id_r_s : in std_logic_vector(UUID_WIDTH-1 downto 0); status_list_goal_info_goal_id_w_s : out std_logic_vector(UUID_WIDTH-1 downto 0); status_list_goal_info_stamp_r_s : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0); status_list_goal_info_stamp_w_s : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0); status_list_status_r_s : in std_logic_vector(CDR_INT8_WIDTH-1 downto 0); status_list_status_w_s : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0); -- *USER* start : in std_logic; opcode : in ROS_ACTION_OPCODE_TYPE; ack : out std_logic; done : out std_logic; return_code : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); goal_handle_in : in std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); goal_handle_out : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); goal_state_in : in std_logic_vector(CDR_INT8_WIDTH-1 downto 0); goal_state_out : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0); goal_id : out std_logic_vector(UUID_WIDTH-1 downto 0); goal_result_index : out std_logic_vector(WORD_WIDTH-1 downto 0); goal_stamp : out ROS_TIME_TYPE; -- GOAL new_goal_request : out std_logic; new_goal_handle : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); new_goal_result_index : out std_logic_vector(WORD_WIDTH-1 downto 0); new_goal_accepted : in std_logic; new_goal_response : in std_logic; -- CANCEL cancel_request : out std_logic; cancel_request_handle : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); cancel_accepted : in std_logic; cancel_response : in std_logic ); end entity; architecture arch of ros_action_server is --*****CONSTANT DECLARATION***** -- *GOAL MEMORY* -- 4-Byte Word Size of a Goal Entry in Memory function gen_goal_frame_size(TIMEOUT_DURATION : ROS_DURATION_TYPE) return natural is begin if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then return 12; else return 10; end if; end function; constant GOAL_FRAME_SIZE : natural := gen_goal_frame_size(TIMEOUT_DURATION); -- Goal Memory Size in 4-Byte Words constant GOAL_MEMORY_SIZE : natural := MAX_GOALS * GOAL_FRAME_SIZE; -- Goal Memory Address Width constant GOAL_MEMORY_ADDR_WIDTH : natural := log2c(GOAL_MEMORY_SIZE); -- Highest Goal Memory Address constant GOAL_MEMORY_MAX_ADDRESS : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0) := to_unsigned(GOAL_MEMORY_SIZE-1, GOAL_MEMORY_ADDR_WIDTH); -- Highest Goal Frame Address constant MAX_GOAL_ADDRESS : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0) := GOAL_MEMORY_MAX_ADDRESS - GOAL_FRAME_SIZE + 1; -- Address pointing to the beginning of the first Goal Data Frame constant FIRST_GOAL_ADDRESS : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0'); -- *RESULT REQUEST MEMORY* -- 4-Byte Word Size of a Result Request Entry in Memory constant RRQ_FRAME_SIZE : natural := 9; -- Result Request Memory Size in 4-Byte Words constant RRQ_MEMORY_SIZE : natural := MAX_RESULT_REQUESTS * RRQ_FRAME_SIZE; -- Result Request Memory Address Width constant RRQ_MEMORY_ADDR_WIDTH : natural := log2c(RRQ_MEMORY_SIZE); -- Highest Result Request Memory Address constant RRQ_MEMORY_MAX_ADDRESS : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0) := to_unsigned(RRQ_MEMORY_SIZE-1, RRQ_MEMORY_ADDR_WIDTH); -- Highest Result Request Frame Address constant MAX_RRQ_ADDRESS : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0) := RRQ_MEMORY_MAX_ADDRESS - RRQ_FRAME_SIZE + 1; -- Address pointing to the beginning of the first Result Request Data Frame constant FIRST_RRQ_ADDRESS : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0'); -- *GOAL MEMORY FRAME FIELD OFFSETS* -- 4-Byte Word Offsets to Beginning of Respective Fields in the Goal Memory Frame constant GMF_STATE_OFFSET : natural := 0; constant GMF_GOAL_ID_OFFSET : natural := 1; constant GMF_STAMP_OFFSET : natural := 5; constant GMF_DEADLINE_OFFSET : natural := 7; function gen_gmf_result_index_offset(TIMEOUT_DURATION : ROS_DURATION_TYPE) return natural is variable ret : natural := 0; begin if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then ret := GMF_DEADLINE_OFFSET + 2; else ret := GMF_DEADLINE_OFFSET; end if; return ret; end function; constant GMF_RESULT_INDEX_OFFSET : natural := gen_gmf_result_index_offset(TIMEOUT_DURATION); constant GMF_NEXT_ADDR_OFFSET : natural := GMF_RESULT_INDEX_OFFSET + 1; constant GMF_PREV_ADDR_OFFSET : natural := GMF_NEXT_ADDR_OFFSET + 1; -- *RESULT REQUEST MEMORY FRAME FIELD OFFSETS* -- 4-Byte Word Offsets to Beginning of Respective Fields in the Result Request Memory Frame constant RMF_GOAL_HANDLE_OFFSET : natural := 0; constant RMF_REQUEST_ID_OFFSET : natural := 1; constant RMF_NEXT_ADDR_OFFSET : natural := 7; constant RMF_PREV_ADDR_OFFSET : natural := 8; -- *GOAL MEMORY FRAME FIELD FLAGS* -- Flags mapping to the respective Goal Memory Frame Fields constant GMF_FLAG_WIDTH : natural := 7; constant GMF_STATE_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (0 => '1', others => '0'); constant GMF_GOAL_ID_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (1 => '1', others => '0'); constant GMF_STAMP_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (2 => '1', others => '0'); constant GMF_DEADLINE_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (3 => '1', others => '0'); constant GMF_RESULT_INDEX_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (4 => '1', others => '0'); constant GMF_NEXT_ADDR_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (5 => '1', others => '0'); constant GMF_PREV_ADDR_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (6 => '1', others => '0'); --*****TYPE DECLARATION***** -- *Cancel Process Search Types* -- SEARCH_GOAL_ID Search goal list for a specific goal id -- SEARCH_STAMP Search goal list for first goal that was accepted at or before a provided timestamp -- SEARCH_NONE No search criteria. All goals are valid 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,GET_S_DATA,PUSH_STATUS,PUBLISH_STATUS,WAIT_FOR_S_PUBLISHER,TRANSITION_STATE,GET_R_DATA,CHECK_REQUESTS,GET_REQUEST_ID,REMOVE_REQUEST,WAIT_FOR_U_MEM,RETURN_USER,PUBLISH_FEEDBACK,WAIT_FOR_G_DATA, WAIT_FOR_G_USER,ADD_GOAL,SEND_G_RESPONSE,WAIT_FOR_G_RET,WAIT_FOR_R_DATA,RESULT_CHECK,STORE_RRQ,SEND_R_RESPONSE,WAIT_FOR_R_RET,WAIT_FOR_C_DATA,GET_C_DATA,CANCEL_CHECK,WAIT_FOR_C_USER,CANCEL_GOAL,ADD_CANCEL,SEND_C_RESPONSE, WAIT_FOR_C_RET,CHECK_TIMEOUT,REMOVE_OLDEST,RESET_MEMORY); type MEM_STAGE_TYPE is (IDLE,INSERT,GET,GET_NEXT,GET_PREV,UPDATE,GET_RESULT_INDEX,REMOVE,RESET_MEMORY); type GOAL_DATA_TYPE is record addr : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); goal_id : GUID_TYPE; stamp : ROS_TIME_TYPE; deadline : ROS_DURATION_TYPE; res_ind : std_logic_vector(WORD_WIDTH-1 downto 0); state : std_logic_vector(CDR_INT8_WIDTH-1 downto 0); field_flags : std_logic_vector(0 to GMF_FLAG_WIDTH-1); end record; constant ZERO_GOAL_DATA : GOAL_DATA_TYPE := ( addr => (others => '0'), goal_id => GUID_UNKNOWN, stamp => ROS_TIME_ZERO, deadline => ROS_DURATION_INFINITE, res_ind => (others => '0'), state => GoalStatus_package.STATUS_UNKNOWN, field_flags => (others => '0') ); --*****SIGNAL DECLARATION***** -- *GOAL MEMORY CONNECTION SIGNALS* signal mem_addr : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); signal mem_read : std_logic; signal mem_read_data, mem_write_data : std_logic_vector(WORD_WIDTH-1 downto 0); signal mem_ready_in, mem_valid_in : std_logic; signal mem_ready_out, mem_valid_out : std_logic; signal mem_abort_read : std_logic; -- *RESULT REQUEST MEMORY CONNECTION SIGNALS* signal rrq_addr : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0); signal rrq_read : std_logic; signal rrq_read_data, rrq_write_data : std_logic_vector(WORD_WIDTH-1 downto 0); signal rrq_ready_in, rrq_valid_in : std_logic; signal rrq_ready_out, rrq_valid_out : std_logic; signal rrq_abort_read : std_logic; -- *MAIN PROCESS* -- FSM state signal stage, stage_next : STAGE_TYPE; -- Request memory operation signal mem_start : std_logic; -- Opcode of goal memory operation (valid only if mem_start is high) signal mem_opcode : MEM_OPCODE_TYPE; -- Signal used to pass data to Goal Memory Process signal mem_r : GOAL_DATA_TYPE; -- Goal memory data latch signal mem_data_r, mem_data_r_next : GOAL_DATA_TYPE; -- Head of occupied result requests list signal rrq_occupied_head, rrq_occupied_head_next : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0); -- Head of empty result requests list signal rrq_empty_head, rrq_empty_head_next : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0); -- General purpose counter signal cnt, cnt_next : natural range 0 to 11; -- Pointer to current relevant Result Request Memory Frame signal rrq_addr_base, rrq_addr_base_next : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0); -- Result Request Memory Frame pointer latch signal rrq_addr_latch, rrq_addr_latch_next : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0); -- Count of stored goals that are in a terminal state signal terminal_cnt, terminal_cnt_next : natural range 0 to MAX_GOALS; -- User return code latch signal return_code_latch, return_code_latch_next : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); -- Toggle latch used to trigger stored result request search when a goal is updated to a terminal state signal trigger_result, trigger_result_next : std_logic; -- Result service Request ID latch signal request_id_latch, request_id_latch_next : REQUEST_ID_TYPE; -- Time of next goal expiration 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; -- Goal handle latch signal goal_handle_latch, goal_handle_latch_next : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0); -- Goal state latch signal goal_state_latch, goal_state_latch_next :std_logic_vector(CDR_INT8_WIDTH-1 downto 0); -- Goal timestamp latch signal g_stamp, g_stamp_next : ROS_TIME_TYPE; -- Goal accept latch signal g_accept, g_accept_next : std_logic; -- Signals the kind of goal cancel search the FSM is doing (see SEARCH_TYPE definition) signal search_type_c, search_type_c_next : SEARCH_TYPE; -- Index of goals_canceling sequence signal goals_canceling_cnt, goals_canceling_cnt_next : natural range 0 to CancelGoal_package.RR_GOALS_CANCELING_MAX_DEPTH-1; -- Cancel response return code latch signal cancel_ret_code, cancel_ret_code_next : std_logic_vector(CDR_INT8_WIDTH-1 downto 0); -- Toggle latch that latches status update requests from other processes signal trigger_status, trigger_status_next : std_logic; -- Index of status list sequence signal status_list_cnt, status_list_cnt_next : natural range 0 to MAX_GOALS; -- Test signal used for testbench synchronisation signal idle_sig : std_logic; -- *GOAL MEMORY PROCESS* -- Memory process FSM state signal mem_stage, mem_stage_next : MEM_STAGE_TYPE; -- Head of occupied goal list signal mem_occupied_head, mem_occupied_head_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); -- Tail of occupied goal list signal mem_occupied_tail, mem_occupied_tail_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); -- Head of empty goal list signal mem_empty_head, mem_empty_head_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); -- Tail of empty goal list signal mem_empty_tail, mem_empty_tail_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); -- General purpose counter signal mem_cnt, mem_cnt_next : natural range 0 to 19; -- Pointer to currently relevant goal memory frame signal mem_addr_base, mem_addr_base_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0); -- Goal memory 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; new_goal_handle <= std_logic_vector(resize(mem_empty_head,GOAL_HANDLE_WIDTH)); new_goal_result_index <= std_logic_vector(empty_head_res_ind); request_id_g <= service_info_g.request_id; accepted_g <= g_accept; stamp_g <= std_logic_vector(to_unsigned(g_stamp)); request_id_c <= service_info_c.request_id; cancel_return_code_c <= cancel_ret_code; goals_canceling_len_c <= std_logic_vector(to_unsigned(goals_canceling_cnt,goals_canceling_len_c'length)); goals_canceling_addr_c <= std_logic_vector(to_unsigned(goals_canceling_cnt,CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH)); goals_canceling_goal_id_w_c <= std_logic_vector(to_unsigned(mem_data_r.goal_id)); goals_canceling_stamp_w_c <= std_logic_vector(to_unsigned(mem_data_r.stamp)); status_list_len_s <= std_logic_vector(to_unsigned(status_list_cnt,status_list_len_s'length)); status_list_addr_s <= std_logic_vector(to_unsigned(status_list_cnt,GoalStatusArray_package.STATUS_LIST_ADDR_WIDTH)); status_list_goal_info_goal_id_w_s <= std_logic_vector(to_unsigned(mem_data_r.goal_id)); status_list_goal_info_stamp_w_s <= std_logic_vector(to_unsigned(mem_data_r.stamp)); status_list_status_w_s <= mem_data_r.state; -- *Main State Machine* -- STATE DESCRIPTION -- IDLE Idle State. Initiates STATUS topic publications, ROS Operation handling, (overflow) Goal removal, GOAL service request handling, RESULT service request handling, CANCEL service request handling, and (timeout) Goal removal, in that priority order. -- GET_S_DATA Get goal data from memory -- PUSH_STATUS Add goal data to status list -- PUBLISH_STATUS Publish status list -- WAIT_FOR_S_PUBLISHER Wait for status list publisher -- TRANSITION_STATE Update goal state -- GET_R_DATA Get goal data required for result response -- CHECK_REQUESTS Search if updated goal is in stored result requests -- GET_REQUEST_ID Get data from stored result request -- REMOVE_REQUEST Remove stored result request -- WAIT_FOR_U_MEM Wait for goal data from memory -- RETURN_USER User return -- PUBLISH_FEEDBACK Publish feedback -- WAIT_FOR_G_DATA Wait goal service request -- WAIT_FOR_G_USER Wait for user input (new_goal_response) -- ADD_GOAL Add goal to memory -- SEND_G_RESPONSE Send goal service response -- WAIT_FOR_G_RET Wait for goal service response -- WAIT_FOR_R_DATA Wait for result service request -- RESULT_CHECK Search for result request goal -- STORE_RRQ Store result request in memory -- SEND_R_RESPONSE Send result service response -- WAIT_FOR_R_RET Wait for result service response -- WAIT_FOR_C_DATA Wait for cancel service request -- GET_C_DATA Get goal data from memory -- CANCEL_CHECK Search for goal cancel targets -- WAIT_FOR_C_USER Wait for user input (cancel_response) -- CANCEL_GOAL Update goal state to CANCELING -- ADD_CANCEL Store canceling goal for later response -- SEND_C_RESPONSE Send cancel service response -- WAIT_FOR_C_RET Wait for cancel service response -- CHECK_TIMEOUT Check and remove expired goals -- REMOVE_OLDEST Remove oldest goal in terminal state -- RESET_MEMORY Reset result request memory to empty state main_prc : process(all) begin -- DEFAULT stage_next <= stage; cnt_next <= cnt; mem_data_r_next <= mem_data_r; terminal_cnt_next <= terminal_cnt; return_code_latch_next <= return_code_latch; trigger_result_next <= trigger_result; rrq_addr_base_next <= rrq_addr_base; rrq_addr_latch_next <= rrq_addr_latch; rrq_empty_head_next <= rrq_empty_head; rrq_occupied_head_next <= rrq_occupied_head; request_id_latch_next <= request_id_latch; time_latch_next <= time_latch; check_time_next <= check_time; goal_handle_latch_next <= goal_handle_latch; goal_state_latch_next <= goal_state_latch; g_stamp_next <= g_stamp; g_accept_next <= g_accept; trigger_status_next <= trigger_status; status_list_cnt_next <= status_list_cnt; search_type_c_next <= search_type_c; goals_canceling_cnt_next <= goals_canceling_cnt; cancel_ret_code_next <= cancel_ret_code; -- DEFAULT Unregistered idle_sig <= '0'; ack <= '0'; done <= '0'; start_fb <= '0'; rrq_abort_read <= '0'; result_sel <= '0'; mem_start <= '0'; mem_opcode <= NOP; start_r <= '0'; opcode_r <= NOP; opcode_fb <= NOP; mem_r <= ZERO_GOAL_DATA; return_code <= ROS_RET_OK; goal_handle_out <= GOAL_HANDLE_UNKNOWN; start_g <= '0'; opcode_g <= NOP; new_goal_request <= '0'; start_s <= '0'; opcode_s <= NOP; status_list_ren_s <= '0'; status_list_wen_s <= '0'; status_list_ack_s <= '0'; start_c <= '0'; opcode_c <= NOP; cancel_request <= '0'; cancel_request_handle <= GOAL_HANDLE_UNKNOWN; goals_canceling_ren_c <= '0'; goals_canceling_wen_c <= '0'; goals_canceling_ack_c <= '0'; rrq_addr <= (others => '0'); rrq_read <= '0'; rrq_valid_in <= '0'; rrq_write_data <= (others => '0'); rrq_ready_out <= '0'; case (stage) is when IDLE => idle_sig <= '1'; if (trigger_status = '1') then -- NOTE: We wait for memory to make sure the mem_occupied_head is set after initial insert if (mem_done = '1') then -- Trigger Status Resetter trigger_status_next <= '0'; -- Reset status_list_cnt_next <= 0; if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then stage_next <= PUBLISH_STATUS; else stage_next <= GET_S_DATA; cnt_next <= 0; end if; end if; elsif (start = '1') then case (opcode) is when GET_GOAL => -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= GET; mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG; mem_r.addr <= resize(unsigned(goal_handle_in),GOAL_MEMORY_ADDR_WIDTH); ack <= '1'; stage_next <= WAIT_FOR_U_MEM; return_code_latch_next <= ROS_RET_OK; end if; when GET_LAST_GOAL => -- No Goals Available if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then mem_data_r_next.addr <= GOAL_MEMORY_MAX_ADDRESS; ack <= '1'; return_code_latch_next <= ROS_RET_OK; stage_next <= RETURN_USER; else -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= GET; mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG; mem_r.addr <= mem_occupied_head; ack <= '1'; stage_next <= WAIT_FOR_U_MEM; return_code_latch_next <= ROS_RET_OK; end if; end if; when GET_FIRST_GOAL => -- No Goals Available if (mem_occupied_tail = GOAL_MEMORY_MAX_ADDRESS) then mem_data_r_next.addr <= GOAL_MEMORY_MAX_ADDRESS; ack <= '1'; return_code_latch_next <= ROS_RET_OK; stage_next <= RETURN_USER; else -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= GET; mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG; mem_r.addr <= mem_occupied_tail; ack <= '1'; stage_next <= WAIT_FOR_U_MEM; return_code_latch_next <= ROS_RET_OK; end if; end if; when GET_NEXT_GOAL => -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= GET_PREV; mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG; mem_r.addr <= resize(unsigned(goal_handle_in),GOAL_MEMORY_ADDR_WIDTH); ack <= '1'; stage_next <= WAIT_FOR_U_MEM; return_code_latch_next <= ROS_RET_OK; end if; when GET_PREVIOUS_GOAL => -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= GET_NEXT; mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG; mem_r.addr <= resize(unsigned(goal_handle_in),GOAL_MEMORY_ADDR_WIDTH); ack <= '1'; stage_next <= WAIT_FOR_U_MEM; return_code_latch_next <= ROS_RET_OK; end if; when UPDATE_GOAL_STATE => -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= GET; mem_r.field_flags <= GMF_STATE_FLAG; mem_r.addr <= resize(unsigned(goal_handle_in),GOAL_MEMORY_ADDR_WIDTH); -- Latch Input Data goal_handle_latch_next <= goal_handle_in; goal_state_latch_next <= goal_state_in; ack <= '1'; stage_next <= TRANSITION_STATE; cnt_next <= 0; end if; when PUBLISH_FEEDBACK => -- SYNTHESIS GUARD if (ENABLE_FEEDBACK = '1') then -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= GET; mem_r.field_flags <= GMF_GOAL_ID_FLAG; mem_r.addr <= resize(unsigned(goal_handle_in),GOAL_MEMORY_ADDR_WIDTH); ack <= '1'; stage_next <= PUBLISH_FEEDBACK; cnt_next <= 0; end if; else ack <= '1'; return_code_latch_next <= ROS_RET_UNSUPPORTED; stage_next <= RETURN_USER; end if; when others => ack <= '1'; return_code_latch_next <= ROS_RET_UNSUPPORTED; stage_next <= RETURN_USER; end case; elsif (TIMEOUT_DURATION = ROS_DURATION_INFINITE and mem_empty_head = GOAL_MEMORY_MAX_ADDRESS and terminal_cnt /= 0 and data_available_g = '1') then assert (mem_occupied_tail /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; mem_data_r_next.addr <= mem_occupied_tail; stage_next <= REMOVE_OLDEST; cnt_next <= 0; elsif (data_available_g = '1' and mem_empty_head /= GOAL_MEMORY_MAX_ADDRESS) then start_g <= '1'; opcode_g <= TAKE_REQUEST; if (ack_g = '1') then stage_next <= WAIT_FOR_G_DATA; end if; elsif (data_available_r = '1' and rrq_empty_head /= RRQ_MEMORY_MAX_ADDRESS) then start_r <= '1'; opcode_r <= TAKE_REQUEST; if (ack_r = '1') then stage_next <= WAIT_FOR_R_DATA; end if; elsif (data_available_c = '1') then start_c <= '1'; opcode_c <= TAKE_REQUEST; if (ack_c = '1') then stage_next <= WAIT_FOR_C_DATA; --Reset goals_canceling_cnt_next <= 0; end if; elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and time >= check_time) then assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; -- Reset check_time_next <= ROS_TIME_INFINITE; mem_data_r_next.addr <= mem_occupied_head; stage_next <= CHECK_TIMEOUT; cnt_next <= 0; end if; when GET_S_DATA => case (cnt) is -- GET FIRST when 0 => assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= GET; mem_r.addr <= mem_occupied_head; mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG; cnt_next <= cnt + 2; end if; -- GET NEXT when 1 => -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= GET_NEXT; mem_r.addr <= mem_data_r.addr; mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG; cnt_next <= cnt + 1; end if; -- WAIT FROM MEM when 2 => -- Wait for DATA if (mem_done = '1') then mem_data_r_next <= mem_data; -- No more Goals if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then stage_next <= PUBLISH_STATUS; else stage_next <= PUSH_STATUS; end if; end if; when others => null; end case; when PUSH_STATUS => status_list_wen_s <= '1'; if (status_list_ready_s = '1') then status_list_cnt_next <= status_list_cnt + 1; if (status_list_cnt = GoalStatusArray_package.STATUS_LIST_MAX_DEPTH-1) then stage_next <= PUBLISH_STATUS; else stage_next <= GET_S_DATA; cnt_next <= 1; -- GET_NEXT end if; end if; when PUBLISH_STATUS => start_s <= '1'; opcode_s <= PUBLISH; if (ack_s = '1') then stage_next <= WAIT_FOR_S_PUBLISHER; end if; when WAIT_FOR_S_PUBLISHER => if (done_s = '1') then -- TODO: Propagate Error? stage_next <= IDLE; end if; when TRANSITION_STATE => case (cnt) is -- CHECK Goal State when 0 => -- Wait for Memory if (mem_done = '1') then assert check_mask(mem_data.field_flags, GMF_STATE_FLAG) severity FAILURE; case (mem_data.state) is when GoalStatus_package.STATUS_ACCEPTED => case (goal_state_latch) is when GoalStatus_package.STATUS_EXECUTING => -- Synthesis Guard if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_latch)) then cnt_next <= cnt + 1; else cnt_next <= cnt + 3; -- UPDATE Goal State end if; when others => stage_next <= RETURN_USER; return_code_latch_next <= ROS_RET_ACTION_GOAL_EVENT_INVALID; end case; when GoalStatus_package.STATUS_EXECUTING => case (goal_state_latch) is when GoalStatus_package.STATUS_SUCCEEDED => -- Synthesis Guard if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_latch)) then cnt_next <= cnt + 1; else cnt_next <= cnt + 3; -- UPDATE Goal State end if; when GoalStatus_package.STATUS_ABORTED => -- Synthesis Guard if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_latch)) then cnt_next <= cnt + 1; else cnt_next <= cnt + 3; -- UPDATE Goal State end if; when others => stage_next <= RETURN_USER; return_code_latch_next <= ROS_RET_ACTION_GOAL_EVENT_INVALID; end case; when GoalStatus_package.STATUS_CANCELING => case (goal_state_latch) is when GoalStatus_package.STATUS_SUCCEEDED => -- Synthesis Guard if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_latch)) then cnt_next <= cnt + 1; else cnt_next <= cnt + 3; -- UPDATE Goal State end if; when GoalStatus_package.STATUS_ABORTED => -- Synthesis Guard if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_latch)) then cnt_next <= cnt + 1; else cnt_next <= cnt + 3; -- UPDATE Goal State end if; when GoalStatus_package.STATUS_CANCELED => -- Synthesis Guard if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_latch)) then cnt_next <= cnt + 1; else cnt_next <= cnt + 3; -- UPDATE Goal State end if; when others => stage_next <= RETURN_USER; return_code_latch_next <= ROS_RET_ACTION_GOAL_EVENT_INVALID; end case; when GoalStatus_package.STATUS_UNKNOWN => -- NOTE: All goals in the empty list have state set to STATUS_UNKNOWN stage_next <= RETURN_USER; return_code_latch_next <= ROS_RET_ACTION_GOAL_HANDLE_INVALID; when others => stage_next <= RETURN_USER; return_code_latch_next <= ROS_RET_ACTION_GOAL_EVENT_INVALID; end case; end if; -- Time Addition 1/2 when 1 => -- Synthesis Guard if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then time_latch_next.sec <= std_logic_vector(unsigned(time.sec) + unsigned(TIMEOUT_DURATION.sec)); time_latch_next.nanosec <= std_logic_vector(unsigned(time.nanosec) + unsigned(TIMEOUT_DURATION.nanosec)); cnt_next <= cnt + 1; end if; -- Time Addition 2/2 when 2 => -- Synthesis Guard if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then if (unsigned(time_latch.nanosec) >= to_unsigned(10**9,time_latch.nanosec'length)) then time_latch_next.sec <= std_logic_vector(unsigned(time_latch.sec) + 1); time_latch_next.nanosec <= std_logic_vector(unsigned(time_latch.nanosec) - to_unsigned(10**9,time_latch.nanosec'length)); end if; cnt_next <= cnt + 1; end if; -- UPDATE Goal State when 3 => -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= UPDATE; mem_r.field_flags <= GMF_STATE_FLAG; mem_r.addr <= resize(unsigned(goal_handle_latch),GOAL_MEMORY_ADDR_WIDTH); mem_r.state <= goal_state_latch; -- Synthesis Guard if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and is_terminal(goal_state_latch)) then mem_r.field_flags <= GMF_STATE_FLAG or GMF_DEADLINE_FLAG; mem_r.deadline <= time_latch; -- Update Checktime if (time_latch < check_time) then check_time_next <= time_latch; end if; end if; if (is_terminal(goal_state_latch)) then trigger_result_next <= '1'; -- Update Terminal Goal Counter terminal_cnt_next <= terminal_cnt + 1; end if; stage_next <= RETURN_USER; return_code_latch_next <= ROS_RET_OK; -- Trigger Status Update trigger_status_next <= '1'; end if; when others => null; end case; when GET_R_DATA => assert (rrq_occupied_head /= RRQ_MEMORY_MAX_ADDRESS) severity FAILURE; case (cnt) is -- GET Data when 0 => -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= GET; mem_r.field_flags <= GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG; mem_r.addr <= resize(unsigned(goal_handle_latch),GOAL_MEMORY_ADDR_WIDTH); cnt_next <= cnt + 1; end if; -- READ Data when 1 => -- Wait for Memory if (mem_done = '1') then assert check_mask(mem_data.field_flags, GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG) severity FAILURE; assert (is_terminal(mem_data.state)) severity FAILURE; mem_data_r_next <= mem_data; if (mem_data.state /= GoalStatus_package.STATUS_SUCCEEDED) then -- NOTE: We are setting result index to MAX_GOALS to effectively "zero" the result. -- This prevents sending the result of previous succeeded goals, if the current -- goal is not succeeded. mem_data_r_next.res_ind <= std_logic_vector(to_unsigned(MAX_GOALS, WORD_WIDTH)); end if; rrq_addr_base_next <= rrq_occupied_head; stage_next <= CHECK_REQUESTS; cnt_next <= 0; end if; when others => null; end case; when CHECK_REQUESTS => case (cnt) is -- GET Goal Handle when 0 => -- NOTE: We check the base here, because a REMOVE_REQUEST operation may reset the rrq_addr_base if (rrq_addr_base = RRQ_MEMORY_MAX_ADDRESS) then -- Reset trigger_result_next <= '0'; -- DONE stage_next <= IDLE; else rrq_valid_in <= '1'; rrq_addr <= rrq_addr_base + RMF_GOAL_HANDLE_OFFSET; rrq_read <= '1'; -- Memory Flow Control Guard if (rrq_ready_in = '1') then cnt_next <= cnt + 1; end if; end if; -- GET Next Addr when 1 => rrq_valid_in <= '1'; rrq_addr <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET; rrq_read <= '1'; -- Memory Flow Control Guard if (rrq_ready_in = '1') then cnt_next <= cnt + 1; end if; -- READ Goal Handle when 2 => rrq_ready_out <= '1'; -- Memory Flow Control Guard if (rrq_valid_out = '1') then -- Found Result Request of Updated Goal if (resize(unsigned(rrq_read_data),GOAL_MEMORY_ADDR_WIDTH) = mem_data_r.addr) then stage_next <= GET_REQUEST_ID; cnt_next <= 0; rrq_abort_read <= '1'; else cnt_next <= cnt + 1; end if; end if; -- READ Next Addr when 3 => rrq_ready_out <= '1'; -- Memory Flow Control Guard if (rrq_valid_out = '1') then rrq_addr_base_next <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH); -- No More Stored Requests if (resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH) = RRQ_MEMORY_MAX_ADDRESS) then -- Reset trigger_result_next <= '0'; stage_next <= IDLE; else -- Next Request cnt_next <= 0; end if; end if; when others => null; end case; when GET_REQUEST_ID => case (cnt) is -- GET Request ID 1/6 when 0 => rrq_valid_in <= '1'; rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET; rrq_read <= '1'; -- Memory Flow Control Guard if (rrq_ready_in = '1') then cnt_next <= cnt + 1; end if; -- GET Request ID 2/6 when 1 => rrq_valid_in <= '1'; rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 1; rrq_read <= '1'; -- Memory Flow Control Guard if (rrq_ready_in = '1') then cnt_next <= cnt + 1; end if; -- GET Request ID 3/6 when 2 => rrq_valid_in <= '1'; rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 2; rrq_read <= '1'; -- Memory Flow Control Guard if (rrq_ready_in = '1') then cnt_next <= cnt + 1; end if; -- GET Request ID 4/6 when 3 => rrq_valid_in <= '1'; rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 3; rrq_read <= '1'; -- Memory Flow Control Guard if (rrq_ready_in = '1') then cnt_next <= cnt + 1; end if; -- GET Request ID 5/6 when 4 => rrq_valid_in <= '1'; rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 4; rrq_read <= '1'; -- Memory Flow Control Guard if (rrq_ready_in = '1') then cnt_next <= cnt + 1; end if; -- GET Request ID 6/6 when 5 => rrq_valid_in <= '1'; rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 5; rrq_read <= '1'; -- Memory Flow Control Guard if (rrq_ready_in = '1') then cnt_next <= cnt + 1; end if; -- READ Request ID 1/6 when 6 => rrq_ready_out <= '1'; -- Memory Flow Control Guard if (rrq_valid_out = '1') then request_id_latch_next.writer_guid(0) <= rrq_read_data; cnt_next <= cnt + 1; end if; -- READ Request ID 2/6 when 7 => rrq_ready_out <= '1'; -- Memory Flow Control Guard if (rrq_valid_out = '1') then request_id_latch_next.writer_guid(1) <= rrq_read_data; cnt_next <= cnt + 1; end if; -- READ Request ID 3/6 when 8 => rrq_ready_out <= '1'; -- Memory Flow Control Guard if (rrq_valid_out = '1') then request_id_latch_next.writer_guid(2) <= rrq_read_data; cnt_next <= cnt + 1; end if; -- READ Request ID 4/6 when 9 => rrq_ready_out <= '1'; -- Memory Flow Control Guard if (rrq_valid_out = '1') then request_id_latch_next.writer_guid(3) <= rrq_read_data; cnt_next <= cnt + 1; end if; -- READ Request ID 5/6 when 10 => rrq_ready_out <= '1'; -- Memory Flow Control Guard if (rrq_valid_out = '1') then request_id_latch_next.sequence_number(0) <= unsigned(rrq_read_data); cnt_next <= cnt + 1; end if; -- READ Request ID 6/6 when 11 => rrq_ready_out <= '1'; -- Memory Flow Control Guard if (rrq_valid_out = '1') then request_id_latch_next.sequence_number(1) <= unsigned(rrq_read_data); stage_next <= REMOVE_REQUEST; cnt_next <= 0; end if; when others => null; end case; when REMOVE_REQUEST => -- NOTE: After this stage the rrq_addr_base is set to the Next request case (cnt) is -- GET Next Addr when 0 => rrq_valid_in <= '1'; rrq_addr <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET; rrq_read <= '1'; -- Memory Flow Control Guard if (rrq_ready_in = '1') then cnt_next <= cnt + 1; end if; -- GET Prev Addr when 1 => rrq_valid_in <= '1'; rrq_addr <= rrq_addr_base + RMF_PREV_ADDR_OFFSET; rrq_read <= '1'; -- Memory Flow Control Guard if (rrq_ready_in = '1') then cnt_next <= cnt + 1; end if; -- SET Next Addr when 2 => rrq_valid_in <= '1'; rrq_addr <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET; rrq_write_data <= std_logic_vector(resize(rrq_empty_head,WORD_WIDTH)); -- Memory Flow Control Guard if (rrq_ready_in = '1') then -- Set New Empty Head rrq_empty_head_next <= rrq_addr_base; cnt_next <= cnt + 1; end if; -- READ Next Addr when 3 => rrq_ready_out <= '1'; -- Memory Flow Control Guard if (rrq_valid_out = '1') then rrq_addr_latch_next <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH); cnt_next <= cnt + 1; end if; -- READ Previous Addr when 4 => rrq_ready_out <= '1'; -- Memory Flow Control Guard if (rrq_valid_out = '1') then if (rrq_addr_latch = RRQ_MEMORY_MAX_ADDRESS) then if (resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH) = RRQ_MEMORY_MAX_ADDRESS) then -- Reset Occupied Head rrq_occupied_head_next <= RRQ_MEMORY_MAX_ADDRESS; rrq_addr_base_next <= RRQ_MEMORY_MAX_ADDRESS; stage_next <= SEND_R_RESPONSE; else rrq_addr_base_next <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH); cnt_next <= cnt + 2; -- Skip Next Slot SET end if; else rrq_addr_base_next <= rrq_addr_latch; rrq_addr_latch_next <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH); cnt_next <= cnt + 1; end if; end if; -- SET Previous Addr (Next Slot) when 5 => rrq_valid_in <= '1'; rrq_addr <= rrq_addr_base + RMF_PREV_ADDR_OFFSET; rrq_write_data <= std_logic_vector(resize(rrq_addr_latch,WORD_WIDTH)); -- Memory Flow Control Guard if (rrq_ready_in = '1') then if (rrq_addr_latch = RRQ_MEMORY_MAX_ADDRESS) then -- Set New Occupied Head rrq_occupied_head_next <= rrq_addr_base; stage_next <= SEND_R_RESPONSE; else rrq_addr_base_next <= rrq_addr_latch; rrq_addr_latch_next <= rrq_addr_base; cnt_next <= cnt + 1; end if; end if; -- SET Next Addr (Previous Slot) when 6 => rrq_valid_in <= '1'; rrq_addr <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET; rrq_write_data <= std_logic_vector(resize(rrq_addr_latch,WORD_WIDTH)); -- Memory Flow Control Guard if (rrq_ready_in = '1') then rrq_addr_base_next <= rrq_addr_latch; stage_next <= SEND_R_RESPONSE; end if; when others => null; end case; when WAIT_FOR_U_MEM => -- Wait for Memory if (mem_done = '1') then assert check_mask(mem_data.field_flags, GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG) severity FAILURE; mem_data_r_next <= mem_data; if (mem_data.addr /= GOAL_MEMORY_MAX_ADDRESS and mem_data.state = GoalStatus_package.STATUS_UNKNOWN) then return_code_latch_next <= ROS_RET_ACTION_GOAL_HANDLE_INVALID; end if; stage_next <= RETURN_USER; end if; when RETURN_USER => -- End of Memory if (mem_data_r.addr = GOAL_MEMORY_MAX_ADDRESS) then goal_handle_out <= GOAL_HANDLE_UNKNOWN; else goal_handle_out <= std_logic_vector(resize(mem_data_r.addr, GOAL_HANDLE_WIDTH)); end if; done <= '1'; return_code <= return_code_latch; if (trigger_result = '1') then -- No stored Result Requests if (rrq_occupied_head = RRQ_MEMORY_MAX_ADDRESS) then -- Reset trigger_result_next <= '0'; stage_next <= IDLE; else stage_next <= GET_R_DATA; cnt_next <= 0; end if; else stage_next <= IDLE; end if; when PUBLISH_FEEDBACK => case (cnt) is -- Wait for Data when 0 => if (mem_done = '1') then assert check_mask(mem_data.field_flags, GMF_GOAL_ID_FLAG) severity FAILURE; mem_data_r_next <= mem_data; cnt_next <= cnt + 1; end if; -- Start Publish when 1 => start_fb <= '1'; opcode_fb <= PUBLISH; if (ack_fb = '1') then cnt_next <= cnt + 1; end if; -- Wait for Publish when 2 => -- Passthrough done <= done_fb; return_code <= return_code_fb; if (done_fb = '1') then -- DONE stage_next <= IDLE; end if; when others => null; end case; when WAIT_FOR_G_DATA => if (done_g = '1') then case (return_code_g) is when ROS_RET_OK => if (taken_g = '1') then stage_next <= WAIT_FOR_G_USER; else stage_next <= IDLE; end if; when others => -- TODO: Propagate Error? stage_next <= IDLE; end case; end if; when WAIT_FOR_G_USER => new_goal_request <= '1'; if (new_goal_response = '1') then g_stamp_next <= time; if (new_goal_accepted = '1') then stage_next <= ADD_GOAL; g_accept_next <= '1'; else stage_next <= SEND_G_RESPONSE; g_accept_next <= '0'; end if; end if; when ADD_GOAL => assert (mem_empty_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= INSERT; stage_next <= SEND_G_RESPONSE; -- Trigger Status Update trigger_status_next <= '1'; end if; when SEND_G_RESPONSE => start_g <= '1'; opcode_g <= SEND_RESPONSE; if (ack_g = '1') then stage_next <= WAIT_FOR_G_RET; end if; when WAIT_FOR_G_RET => if (done_g = '1') then -- TODO: Propagate Error? stage_next <= IDLE; end if; when WAIT_FOR_R_DATA => if (done_r = '1') then case (return_code_r) is when ROS_RET_OK => if (taken_r = '1') then -- Latch Request ID request_id_latch_next <= service_info_r.request_id; -- No Goals Available if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then mem_data_r_next.res_ind <= std_logic_vector(to_unsigned(MAX_GOALS, WORD_WIDTH)); mem_data_r_next.state <= GoalStatus_package.STATUS_UNKNOWN; mem_data_r_next.field_flags <= GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG; stage_next <= SEND_R_RESPONSE; else stage_next <= RESULT_CHECK; cnt_next <= 0; end if; else stage_next <= IDLE; end if; when others => -- TODO: Propagate Error? stage_next <= IDLE; end case; end if; when RESULT_CHECK => case (cnt) is -- GET FIRST when 0 => assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= GET; mem_r.field_flags <= GMF_GOAL_ID_FLAG; mem_r.addr <= mem_occupied_head; cnt_next <= cnt + 2; end if; -- GET NEXT when 1 => -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= GET_NEXT; mem_r.field_flags <= GMF_GOAL_ID_FLAG; mem_r.addr <= mem_data_r.addr; cnt_next <= cnt + 1; end if; -- CHECK when 2 => -- Wait for Memory if (mem_done = '1') then assert check_mask(mem_data.field_flags, GMF_GOAL_ID_FLAG) severity FAILURE; -- No more Goals if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then -- Goal not found mem_data_r_next.res_ind <= std_logic_vector(to_unsigned(MAX_GOALS, WORD_WIDTH)); mem_data_r_next.state <= GoalStatus_package.STATUS_UNKNOWN; mem_data_r_next.field_flags <= GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG; stage_next <= SEND_R_RESPONSE; else mem_data_r_next <= mem_data; cnt_next <= cnt + 1; end if; end if; -- CHECK GOAL_ID 1/4 when 3 => if (mem_data_r.goal_id(0) = to_GUID(goal_id_r)(0)) then cnt_next <= cnt + 1; else cnt_next <= 1; -- GET_NEXT end if; -- CHECK GOAL_ID 2/4 when 4 => if (mem_data_r.goal_id(1) = to_GUID(goal_id_r)(1)) then cnt_next <= cnt + 1; else cnt_next <= 1; -- GET_NEXT end if; -- CHECK GOAL_ID 3/4 when 5 => if (mem_data_r.goal_id(2) = to_GUID(goal_id_r)(2)) then cnt_next <= cnt + 1; else cnt_next <= 1; -- GET_NEXT end if; -- CHECK GOAL_ID 4/4 when 6 => if (mem_data_r.goal_id(3) = to_GUID(goal_id_r)(3)) then -- Found Goal cnt_next <= cnt + 1; else cnt_next <= 1; -- GET_NEXT end if; -- GET STATE when 7 => -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= GET; mem_r.field_flags <= GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG; mem_r.addr <= mem_data_r.addr; cnt_next <= cnt + 1; end if; -- Check State when 8 => -- Wait for Memory if (mem_done = '1') then assert check_mask(mem_data.field_flags, GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG) severity FAILURE; mem_data_r_next <= mem_data; if (is_terminal(mem_data.state)) then if (mem_data.state /= GoalStatus_package.STATUS_SUCCEEDED) then -- NOTE: We are setting result index to MAX_GOALS to effectively "zero" the result. -- This prevents sending the result of previous succeeded goals, if the current -- goal is not succeeded. mem_data_r_next.res_ind <= std_logic_vector(to_unsigned(MAX_GOALS, WORD_WIDTH)); end if; stage_next <= SEND_R_RESPONSE; else stage_next <= STORE_RRQ; cnt_next <= 0; end if; end if; when others => null; end case; when STORE_RRQ => assert (rrq_empty_head /= RRQ_MEMORY_MAX_ADDRESS) severity FAILURE; case (cnt) is -- GET Next Addr when 0 => rrq_valid_in <= '1'; rrq_addr <= rrq_empty_head + RMF_NEXT_ADDR_OFFSET; rrq_read <= '1'; -- Memory Flow Control Guard if (rrq_ready_in = '1') then cnt_next <= cnt + 1; end if; -- SET Goal Handle when 1 => rrq_valid_in <= '1'; rrq_addr <= rrq_empty_head + RMF_GOAL_HANDLE_OFFSET; rrq_write_data <= std_logic_vector(resize(mem_data_r.addr,WORD_WIDTH)); -- Memory Flow Control Guard if (rrq_ready_in = '1') then cnt_next <= cnt + 1; end if; -- SET Request ID 1/6 when 2 => rrq_valid_in <= '1'; rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET; rrq_write_data <= request_id_latch.writer_guid(0); -- Memory Flow Control Guard if (rrq_ready_in = '1') then cnt_next <= cnt + 1; end if; -- SET Request ID 2/6 when 3 => rrq_valid_in <= '1'; rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 1; rrq_write_data <= request_id_latch.writer_guid(1); -- Memory Flow Control Guard if (rrq_ready_in = '1') then cnt_next <= cnt + 1; end if; -- SET Request ID 3/6 when 4 => rrq_valid_in <= '1'; rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 2; rrq_write_data <= request_id_latch.writer_guid(2); -- Memory Flow Control Guard if (rrq_ready_in = '1') then cnt_next <= cnt + 1; end if; -- SET Request ID 4/6 when 5 => rrq_valid_in <= '1'; rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 3; rrq_write_data <= request_id_latch.writer_guid(3); -- Memory Flow Control Guard if (rrq_ready_in = '1') then cnt_next <= cnt + 1; end if; -- SET Request ID 5/6 when 6 => rrq_valid_in <= '1'; rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 4; rrq_write_data <= std_logic_vector(request_id_latch.sequence_number(0)); -- Memory Flow Control Guard if (rrq_ready_in = '1') then cnt_next <= cnt + 1; end if; -- SET Request ID 6/6 when 7 => rrq_valid_in <= '1'; rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 5; rrq_write_data <= std_logic_vector(request_id_latch.sequence_number(1)); -- Memory Flow Control Guard if (rrq_ready_in = '1') then if (rrq_occupied_head = RRQ_MEMORY_MAX_ADDRESS) then cnt_next <= cnt + 2; else cnt_next <= cnt + 1; end if; end if; -- SET Prev Addr (Old Occupied Head) when 8 => assert (rrq_occupied_head /= RRQ_MEMORY_MAX_ADDRESS) severity FAILURE; rrq_valid_in <= '1'; rrq_addr <= rrq_occupied_head + RMF_PREV_ADDR_OFFSET; rrq_write_data <= std_logic_vector(resize(rrq_empty_head,WORD_WIDTH)); -- Memory Flow Control Guard if (rrq_ready_in = '1') then cnt_next <= cnt + 1; end if; -- SET Next Addr when 9 => rrq_valid_in <= '1'; rrq_addr <= rrq_empty_head + RMF_NEXT_ADDR_OFFSET; rrq_write_data <= std_logic_vector(resize(rrq_occupied_head,WORD_WIDTH)); -- Memory Flow Control Guard if (rrq_ready_in = '1') then -- Set New Occupied Head rrq_occupied_head_next <= rrq_empty_head; cnt_next <= cnt + 1; end if; -- SET Prev Addr when 10 => rrq_valid_in <= '1'; rrq_addr <= rrq_empty_head + RMF_PREV_ADDR_OFFSET; rrq_write_data <= std_logic_vector(resize(RRQ_MEMORY_MAX_ADDRESS,WORD_WIDTH)); -- Memory Flow Control Guard if (rrq_ready_in = '1') then cnt_next <= cnt + 1; end if; -- READ Next Addr when 11 => rrq_ready_out <= '1'; -- Memory Flow Control Guard if (rrq_valid_out = '1') then -- Set new Empty Head rrq_empty_head_next <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH); -- DONE stage_next <= IDLE; end if; when others => null; end case; when SEND_R_RESPONSE => assert check_mask(mem_data_r.field_flags, GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG) severity FAILURE; result_sel <= '1'; if (result_sel_ack = '1') then start_r <= '1'; opcode_r <= SEND_RESPONSE; if (ack_r = '1') then stage_next <= WAIT_FOR_R_RET; end if; end if; when WAIT_FOR_R_RET => assert check_mask(mem_data_r.field_flags, GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG) severity FAILURE; result_sel <= '1'; if (done_r = '1') then -- TODO: Propagate Error? stage_next <= IDLE; if (trigger_result = '1') then stage_next <= CHECK_REQUESTS; cnt_next <= 0; end if; end if; when WAIT_FOR_C_DATA => if (done_c = '1') then case (return_code_c) is when ROS_RET_OK => if (taken_c = '1') then -- No Goals Available if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then cancel_ret_code_next <= CancelGoal_package.RR_ERROR_REJECTED; stage_next <= SEND_C_RESPONSE; else if (to_UUID(goal_info_goal_id_c) = UUID_UNKNOWN) then if (to_ROS_TIME(goal_info_stamp_c) = ROS_TIME_ZERO) then search_type_c_next <= SEARCH_NONE; else search_type_c_next <= SEARCH_STAMP; end if; else search_type_c_next <= SEARCH_GOAL_ID; end if; stage_next <= GET_C_DATA; cnt_next <= 0; -- GET_FIRST end if; else stage_next <= IDLE; end if; when others => -- TODO: Propagate Error? stage_next <= IDLE; end case; end if; when GET_C_DATA => case (cnt) is -- GET FIRST when 0 => -- Memory Operation Guard if (mem_done = '1') then assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; mem_start <= '1'; mem_opcode <= GET; mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG; mem_r.addr <= mem_occupied_head; stage_next <= CANCEL_CHECK; cnt_next <= 0; end if; -- GET NEXT when 1 => -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= GET_NEXT; mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG; mem_r.addr <= mem_data_r.addr; stage_next <= CANCEL_CHECK; cnt_next <= 0; end if; when others => null; end case; when CANCEL_CHECK => case (cnt) is -- WAIT FOR MEMORY when 0 => -- Wait for Memory if (mem_done = '1') then -- End of Memory if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then case (search_type_c) is when SEARCH_GOAL_ID => cancel_ret_code_next <= CancelGoal_package.RR_ERROR_UNKNOWN_GOAL_ID; stage_next <= SEND_C_RESPONSE; when others => -- No Goals Canceled if (goals_canceling_cnt = 0) then cancel_ret_code_next <= CancelGoal_package.RR_ERROR_REJECTED; stage_next <= SEND_C_RESPONSE; else cancel_ret_code_next <= CancelGoal_package.RR_ERROR_NONE; stage_next <= SEND_C_RESPONSE; end if; end case; else assert check_mask(mem_data.field_flags, GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG) severity FAILURE; mem_data_r_next <= mem_data; case (search_type_c) is when SEARCH_GOAL_ID => cnt_next <= 1; -- CHECK GOAL_ID when SEARCH_STAMP => cnt_next <= 5; -- CHECK STAMP when SEARCH_NONE => cnt_next <= 6; -- CHECK STATE end case; end if; end if; -- CHECK GOAL_ID 1/4 when 1 => if (mem_data_r.goal_id(0) = to_guid(goal_info_goal_id_c)(0)) then cnt_next <= cnt + 1; else stage_next <= GET_C_DATA; cnt_next <= 1; -- GET_NEXT end if; -- CHECK GOAL_ID 2/4 when 2 => if (mem_data_r.goal_id(1) = to_guid(goal_info_goal_id_c)(1)) then cnt_next <= cnt + 1; else stage_next <= GET_C_DATA; cnt_next <= 1; -- GET_NEXT end if; -- CHECK GOAL_ID 3/4 when 3 => if (mem_data_r.goal_id(2) = to_guid(goal_info_goal_id_c)(2)) then cnt_next <= cnt + 1; else stage_next <= GET_C_DATA; cnt_next <= 1; -- GET_NEXT end if; -- CHECK GOAL_ID 4/4 when 4 => if (mem_data_r.goal_id(3) = to_guid(goal_info_goal_id_c)(3)) then cnt_next <= cnt + 2; else stage_next <= GET_C_DATA; cnt_next <= 1; -- GET_NEXT end if; -- CHECK STAMP when 5 => if (mem_data_r.stamp <= to_ROS_TIME(goal_info_stamp_c)) then cnt_next <= cnt + 1; else stage_next <= GET_C_DATA; cnt_next <= 1; -- GET_NEXT end if; -- CHECK STATE when 6 => if (is_terminal(mem_data_r.state)) then if (search_type_c = SEARCH_GOAL_ID) then cancel_ret_code_next <= CancelGoal_package.RR_ERROR_GOAL_TERMINATED; stage_next <= SEND_C_RESPONSE; else stage_next <= GET_C_DATA; cnt_next <= 1; -- GET_NEXT end if; elsif (mem_data_r.state = GoalStatus_package.STATUS_CANCELING) then -- Skip State Change stage_next <= ADD_CANCEL; else stage_next <= WAIT_FOR_C_USER; end if; when others => null; end case; when WAIT_FOR_C_USER => cancel_request <= '1'; cancel_request_handle <= std_logic_vector(resize(mem_data_r.addr,GOAL_HANDLE_WIDTH)); if (cancel_response = '1') then if (cancel_accepted = '1') then stage_next <= CANCEL_GOAL; else case (search_type_c) is when SEARCH_GOAL_ID => if (to_ROS_TIME(goal_info_stamp_c) = ROS_TIME_ZERO) then -- No Goals Canceled cancel_ret_code_next <= CancelGoal_package.RR_ERROR_REJECTED; stage_next <= SEND_C_RESPONSE; else -- Search for Timestamps search_type_c_next <= SEARCH_STAMP; stage_next <= GET_C_DATA; cnt_next <= 0; -- GET_FIRST end if; when SEARCH_STAMP => -- NOTE: Since the goals are added chronologically, we don't have to check the timestamps from now on search_type_c_next <= SEARCH_NONE; stage_next <= GET_C_DATA; cnt_next <= 1; -- GET_NEXT when SEARCH_NONE => -- Continue stage_next <= GET_C_DATA; cnt_next <= 1; -- GET_NEXT end case; end if; end if; when CANCEL_GOAL => -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= UPDATE; mem_r.addr <= mem_data_r.addr; mem_r.field_flags <= GMF_STATE_FLAG; mem_r.state <= GoalStatus_package.STATUS_CANCELING; stage_next <= ADD_CANCEL; -- Trigger Status Update trigger_status_next <= '1'; end if; when ADD_CANCEL => goals_canceling_wen_c <= '1'; if (goals_canceling_ready_c = '1') then -- Reached maximum number of cancel goals if (goals_canceling_cnt = CancelGoal_package.RR_GOALS_CANCELING_MAX_DEPTH) then -- DONE cancel_ret_code_next <= CancelGoal_package.RR_ERROR_NONE; stage_next <= SEND_C_RESPONSE; else goals_canceling_cnt_next <= goals_canceling_cnt + 1; case (search_type_c) is when SEARCH_GOAL_ID => if (to_ROS_TIME(goal_info_stamp_c) = ROS_TIME_ZERO) then -- DONE cancel_ret_code_next <= CancelGoal_package.RR_ERROR_NONE; stage_next <= SEND_C_RESPONSE; else -- Search for Timestamps search_type_c_next <= SEARCH_STAMP; stage_next <= GET_C_DATA; cnt_next <= 0; -- GET_FIRST end if; when SEARCH_STAMP => -- NOTE: Since the goals are added chronologically, we don't have to check the timestamps from now on search_type_c_next <= SEARCH_NONE; stage_next <= GET_C_DATA; cnt_next <= 1; -- GET_NEXT when SEARCH_NONE => -- Continue stage_next <= GET_C_DATA; cnt_next <= 1; -- GET_NEXT end case; end if; end if; when SEND_C_RESPONSE => start_c <= '1'; opcode_c <= SEND_RESPONSE; if (ack_c = '1') then stage_next <= WAIT_FOR_C_RET; end if; when WAIT_FOR_C_RET => if (done_c = '1') then -- TODO: Propagate Error? stage_next <= IDLE; end if; when CHECK_TIMEOUT => -- Synthesis Guard if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then case (cnt) is -- GET when 0 => -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= GET; mem_r.field_flags <= GMF_DEADLINE_FLAG; mem_r.addr <= mem_data_r.addr; cnt_next <= cnt + 2; end if; -- GET NEXT when 1 => -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= GET_NEXT; mem_r.field_flags <= GMF_DEADLINE_FLAG; mem_r.addr <= mem_data_r.addr; cnt_next <= cnt + 1; end if; -- CHECK when 2 => -- Wait for Memory if (mem_done = '1') then assert check_mask(mem_data.field_flags, GMF_DEADLINE_FLAG) severity FAILURE; mem_data_r_next <= mem_data; -- No more Goals if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then -- DONE stage_next <= IDLE; else -- Timeout if (mem_data.deadline <= time) then cnt_next <= cnt + 1; else -- Update check Time if (mem_data.deadline < check_time) then check_time_next <= mem_data.deadline; end if; cnt_next <= 1; -- GET_NEXT end if; end if; end if; -- REMOVE when 3 => -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= REMOVE; mem_r.addr <= mem_data_r.addr; cnt_next <= cnt + 1; -- Trigger Status Update trigger_status_next <= '1'; -- Update Terminal Goal Counter terminal_cnt_next <= terminal_cnt - 1; end if; -- Wait for Remove when 4 => -- Wait for Memory if (mem_done = '1') then mem_data_r_next <= mem_data; -- NOTE: After Removal, mem_data.addr is set to the next goal, so we need to call GET and not GET_NEXT -- No more Goals if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then -- DONE stage_next <= IDLE; else cnt_next <= 0; -- GET end if; end if; when others => null; end case; end if; when REMOVE_OLDEST => -- Synthesis Guard if (TIMEOUT_DURATION = ROS_DURATION_INFINITE) then assert (terminal_cnt > 0) severity FAILURE; case (cnt) is -- GET when 0 => -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= GET; mem_r.field_flags <= GMF_STATE_FLAG; mem_r.addr <= mem_data_r.addr; cnt_next <= cnt + 2; end if; -- GET NEXT when 1 => -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= GET_PREV; mem_r.field_flags <= GMF_STATE_FLAG; mem_r.addr <= mem_data_r.addr; cnt_next <= cnt + 1; end if; -- CHECK when 2 => -- Wait for Memory if (mem_done = '1') then assert check_mask(mem_data.field_flags, GMF_STATE_FLAG) severity FAILURE; mem_data_r_next <= mem_data; -- No more Goals if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then assert FALSE report "REMOVE_OLDEST did not find goal to remove" severity FAILURE; stage_next <= IDLE; else if (is_terminal(mem_data.state)) then cnt_next <= cnt + 1; else cnt_next <= 1; -- GET_PREV end if; end if; end if; -- REMOVE when 3 => -- Memory Operation Guard if (mem_done = '1') then mem_start <= '1'; mem_opcode <= REMOVE; mem_r.addr <= mem_data_r.addr; -- Trigger Status Update trigger_status_next <= '1'; -- Update Terminal Goal Counter terminal_cnt_next <= terminal_cnt - 1; -- DONE stage_next <= IDLE; end if; when others => null; end case; end if; when RESET_MEMORY => case (cnt) is -- SET Next Pointer when 0 => rrq_valid_in <= '1'; rrq_addr <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET; if (rrq_addr_base = MAX_RRQ_ADDRESS) then rrq_write_data <= std_logic_vector(resize(RRQ_MEMORY_MAX_ADDRESS,WORD_WIDTH)); else rrq_write_data <= std_logic_vector(resize(rrq_addr_base + RRQ_FRAME_SIZE,WORD_WIDTH)); end if; -- Memory Flow Control Guard if (rrq_ready_in = '1') then cnt_next <= cnt + 1; end if; -- SET Previous Pointer when 1 => rrq_valid_in <= '1'; rrq_addr <= rrq_addr_base + RMF_PREV_ADDR_OFFSET; rrq_write_data <= std_logic_vector(resize(rrq_addr_latch,WORD_WIDTH)); -- Memory Flow Control Guard if (rrq_ready_in = '1') then if (rrq_addr_base = MAX_RRQ_ADDRESS) then -- Initialize Empty and Occupied Heads rrq_empty_head_next <= FIRST_RRQ_ADDRESS; rrq_occupied_head_next <= RRQ_MEMORY_MAX_ADDRESS; -- DONE stage_next <= IDLE; else rrq_addr_latch_next <= rrq_addr_base; rrq_addr_base_next <= rrq_addr_base + RRQ_FRAME_SIZE; cnt_next <= 0; end if; end if; when others => null; end case; end case; end process; -- *Goal Memory State Machine* -- STATE DESCRIPTION -- IDLE Idle State. Done Signal is pulled high and Memory FSM accepts new memory operations -- INSERT See Memory OPCODE Description -- GET See Memory OPCODE Description -- GET_NEXT See Memory OPCODE Description -- GET_PREV See Memory OPCODE Description -- UPDATE See Memory OPCODE Description -- GET_RESULT_INDEX Set the empty_head_res_ind before REMOVE -- REMOVE See Memory OPCODE Description -- RESET_MEMORY Reset goal memory to empty state mem_ctrl_prc : process(all) begin -- DEFAULT mem_stage_next <= mem_stage; mem_cnt_next <= mem_cnt; mem_occupied_head_next <= mem_occupied_head; mem_occupied_tail_next <= mem_occupied_tail; mem_empty_head_next <= mem_empty_head; mem_empty_tail_next <= mem_empty_tail; empty_head_res_ind_next <= empty_head_res_ind; mem_latch_data_next <= mem_latch_data; mem_data_next <= mem_data; mem_addr_base_next <= mem_addr_base; mem_addr_latch_next <= mem_addr_latch; -- DEFAULT unregistered mem_done <= '0'; mem_abort_read <= '0'; mem_ready_out <= '0'; mem_valid_in <= '0'; mem_read <= '0'; mem_done <= '0'; mem_addr <= (others => '0'); mem_write_data <= (others => '0'); case (mem_stage) is -- NOTE: The REMOVE opcode sets mem_data.addr to the next slot (or GOAL_MEMORY_MAX_ADDRESS if no next goals) when IDLE => mem_done <= '1'; if (mem_start = '1') then case (mem_opcode) is when INSERT => assert (mem_empty_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; -- Latch DATA mem_latch_data_next.goal_id <= to_guid(goal_id_g); mem_latch_data_next.stamp <= g_stamp; mem_addr_base_next <= mem_empty_head; mem_stage_next <= INSERT; mem_cnt_next <= 0; when GET => assert (mem_r.addr /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; -- Latch DATA mem_latch_data_next <= mem_r; if (mem_r.addr = mem_addr_base) then mem_data_next.field_flags <= mem_data.field_flags or mem_r.field_flags; else mem_data_next <= ZERO_GOAL_DATA; mem_data_next.field_flags <= mem_r.field_flags; end if; mem_data_next.addr <= mem_r.addr; mem_addr_base_next <= mem_r.addr; mem_stage_next <= GET; if check_mask(mem_r.field_flags,GMF_STATE_FLAG) then mem_cnt_next <= 0; elsif check_mask(mem_r.field_flags,GMF_GOAL_ID_FLAG) then mem_cnt_next <= 1; elsif check_mask(mem_r.field_flags,GMF_STAMP_FLAG) then mem_cnt_next <= 5; elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_r.field_flags,GMF_DEADLINE_FLAG)) then mem_cnt_next <= 7; elsif check_mask(mem_r.field_flags,GMF_RESULT_INDEX_FLAG) then mem_cnt_next <= 9; else -- DONE mem_stage_next <= IDLE; end if; when GET_NEXT => assert (mem_r.addr /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; -- Latch DATA mem_latch_data_next <= mem_r; mem_data_next <= ZERO_GOAL_DATA; mem_data_next.field_flags <= mem_r.field_flags; mem_addr_base_next <= mem_r.addr; mem_stage_next <= GET_NEXT; mem_cnt_next <= 0; when GET_PREV => assert (mem_r.addr /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; -- Latch DATA mem_latch_data_next <= mem_r; mem_data_next <= ZERO_GOAL_DATA; mem_data_next.field_flags <= mem_r.field_flags; mem_addr_base_next <= mem_r.addr; mem_stage_next <= GET_PREV; mem_cnt_next <= 0; when UPDATE => assert (mem_r.addr /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; -- Latch DATA mem_latch_data_next <= mem_r; if (mem_r.addr = mem_addr_base) then mem_data_next.field_flags <= mem_data.field_flags or mem_r.field_flags; else mem_data_next <= ZERO_GOAL_DATA; mem_data_next.field_flags <= mem_r.field_flags; end if; mem_data_next.addr <= mem_r.addr; mem_addr_base_next <= mem_r.addr; mem_stage_next <= UPDATE; if check_mask(mem_r.field_flags,GMF_STATE_FLAG) then mem_cnt_next <= 0; elsif check_mask(mem_r.field_flags,GMF_GOAL_ID_FLAG) then mem_cnt_next <= 1; elsif check_mask(mem_r.field_flags,GMF_STAMP_FLAG) then mem_cnt_next <= 5; elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_r.field_flags,GMF_DEADLINE_FLAG)) then mem_cnt_next <= 7; elsif check_mask(mem_r.field_flags,GMF_RESULT_INDEX_FLAG) then mem_cnt_next <= 9; else -- DONE mem_stage_next <= IDLE; end if; when REMOVE => assert (mem_r.addr /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; mem_addr_base_next <= mem_r.addr; if (mem_empty_head = GOAL_MEMORY_MAX_ADDRESS) then mem_stage_next <= GET_RESULT_INDEX; mem_cnt_next <= 0; else mem_stage_next <= REMOVE; mem_cnt_next <= 0; end if; when others => null; end case; end if; when INSERT => case (mem_cnt) is -- GET Next Addr when 0 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET; mem_read <= '1'; -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- READ Next Addr when 1 => mem_ready_out <= '1'; -- Memory Flow Control Guard if (mem_valid_out = '1') then -- SET New Empty Head mem_empty_head_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); if (resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH) = GOAL_MEMORY_MAX_ADDRESS) then assert (mem_empty_head = mem_empty_tail) severity FAILURE; -- SET New Empty Tail mem_empty_tail_next <= GOAL_MEMORY_MAX_ADDRESS; mem_cnt_next <= mem_cnt + 3; -- Skip Result index else mem_cnt_next <= mem_cnt + 1; end if; end if; -- GET Result Index (New Empty Head) when 2 => assert (mem_empty_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; mem_valid_in <= '1'; mem_addr <= mem_empty_head + GMF_RESULT_INDEX_OFFSET; mem_read <= '1'; -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- READ Result Index (New Empty Head) when 3 => mem_ready_out <= '1'; -- Memory Flow Control Guard if (mem_valid_out = '1') then empty_head_res_ind_next <= unsigned(mem_read_data); mem_cnt_next <= mem_cnt + 1; end if; -- SET Goal State when 4 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_STATE_OFFSET; mem_write_data(CDR_INT8_WIDTH-1 downto 0) <= GoalStatus_package.STATUS_ACCEPTED; mem_data_next.state <= GoalStatus_package.STATUS_ACCEPTED; mem_data_next.field_flags <= mem_data.field_flags or GMF_STATE_FLAG; -- 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; mem_data_next.field_flags <= mem_data.field_flags or GMF_GOAL_ID_FLAG; -- 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; mem_data_next.field_flags <= mem_data.field_flags or GMF_STAMP_FLAG; -- 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; mem_data_next.field_flags <= mem_data.field_flags or GMF_DEADLINE_FLAG; -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; end if; -- SET Prev Addr when 13 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_PREV_ADDR_OFFSET; mem_write_data <= std_logic_vector(resize(GOAL_MEMORY_MAX_ADDRESS,WORD_WIDTH)); -- Memory Flow Control Guard if (mem_ready_in = '1') then if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then mem_cnt_next <= mem_cnt + 2; -- Skip next Step else mem_cnt_next <= mem_cnt + 1; end if; end if; -- SET Prev Addr (Current Occupied Head) when 14 => assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; mem_valid_in <= '1'; mem_addr <= mem_occupied_head + GMF_PREV_ADDR_OFFSET; mem_write_data <= std_logic_vector(resize(mem_addr_base,WORD_WIDTH)); -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- SET Next Addr when 15 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET; mem_write_data <= std_logic_vector(resize(mem_occupied_head,WORD_WIDTH)); mem_data_next.addr <= mem_addr_base; -- Memory Flow Control Guard if (mem_ready_in = '1') then -- Update Occupied Head mem_occupied_head_next <= mem_addr_base; -- Initial Occupied Tail if (mem_occupied_tail = GOAL_MEMORY_MAX_ADDRESS) then mem_occupied_tail_next <= mem_addr_base; end if; -- DONE mem_stage_next <= IDLE; end if; when others => null; end case; when GET => case (mem_cnt) is -- GET Goal State when 0 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_STATE_OFFSET; mem_read <= '1'; -- Memory Flow Control Guard if (mem_ready_in = '1') then if check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then mem_cnt_next <= 1; elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then mem_cnt_next <= 5; elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then mem_cnt_next <= 7; elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then mem_cnt_next <= 9; else mem_cnt_next <= 10; end if; end if; -- GET Goal ID 1/4 when 1 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET; mem_read <= '1'; -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- GET Goal ID 2/4 when 2 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 1; mem_read <= '1'; -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- GET Goal ID 3/4 when 3 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 2; mem_read <= '1'; -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- GET Goal ID 4/4 when 4 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 3; mem_read <= '1'; -- Memory Flow Control Guard if (mem_ready_in = '1') then if check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then mem_cnt_next <= 5; elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then mem_cnt_next <= 7; elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then mem_cnt_next <= 9; else if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then mem_cnt_next <= 10; else mem_cnt_next <= 11; end if; end if; end if; -- GET Stamp 1/2 when 5 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_STAMP_OFFSET; mem_read <= '1'; -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- GET Stamp 2/2 when 6 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_STAMP_OFFSET + 1; mem_read <= '1'; -- Memory Flow Control Guard if (mem_ready_in = '1') then if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then mem_cnt_next <= 7; elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then mem_cnt_next <= 9; else if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then mem_cnt_next <= 10; elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then mem_cnt_next <= 11; else mem_cnt_next <= 15; end if; end if; end if; -- GET Deadline 1/2 when 7 => -- Synthesis Guard if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET; mem_read <= '1'; -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; end if; -- GET Deadline 2/2 when 8 => -- Synthesis Guard if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET + 1; mem_read <= '1'; -- Memory Flow Control Guard if (mem_ready_in = '1') then if check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then mem_cnt_next <= 9; else if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then mem_cnt_next <= 10; elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then mem_cnt_next <= 11; elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then mem_cnt_next <= 15; else mem_cnt_next <= 17; end if; end if; end if; end if; -- GET Result Index when 9 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_RESULT_INDEX_OFFSET; mem_read <= '1'; -- Memory Flow Control Guard if (mem_ready_in = '1') then if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then mem_cnt_next <= 10; elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then mem_cnt_next <= 11; elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then mem_cnt_next <= 15; elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then mem_cnt_next <= 17; else mem_cnt_next <= 19; end if; end if; -- READ Goal State when 10 => mem_ready_out <= '1'; -- Memory Flow Control Guard if (mem_valid_out = '1') then mem_data_next.state <= mem_read_data(CDR_INT8_WIDTH-1 downto 0); if check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then mem_cnt_next <= 11; elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then mem_cnt_next <= 15; elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then mem_cnt_next <= 17; elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then mem_cnt_next <= 19; else mem_stage_next <= IDLE; end if; end if; -- READ Goal ID 1/4 when 11 => mem_ready_out <= '1'; -- Memory Flow Control Guard if (mem_valid_out = '1') then mem_data_next.goal_id(0) <= mem_read_data; mem_cnt_next <= mem_cnt + 1; end if; -- READ Goal ID 2/4 when 12 => mem_ready_out <= '1'; -- Memory Flow Control Guard if (mem_valid_out = '1') then mem_data_next.goal_id(1) <= mem_read_data; mem_cnt_next <= mem_cnt + 1; end if; -- READ Goal ID 3/4 when 13 => mem_ready_out <= '1'; -- Memory Flow Control Guard if (mem_valid_out = '1') then mem_data_next.goal_id(2) <= mem_read_data; mem_cnt_next <= mem_cnt + 1; end if; -- READ Goal ID 4/4 when 14 => mem_ready_out <= '1'; -- Memory Flow Control Guard if (mem_valid_out = '1') then mem_data_next.goal_id(3) <= mem_read_data; if check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then mem_cnt_next <= 15; elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then mem_cnt_next <= 17; elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then mem_cnt_next <= 19; else mem_stage_next <= IDLE; end if; end if; -- READ Stamp 1/2 when 15 => mem_ready_out <= '1'; -- Memory Flow Control Guard if (mem_valid_out = '1') then mem_data_next.stamp.sec <= mem_read_data; mem_cnt_next <= mem_cnt + 1; end if; -- READ Stamp 2/2 when 16 => mem_ready_out <= '1'; -- Memory Flow Control Guard if (mem_valid_out = '1') then mem_data_next.stamp.nanosec <= mem_read_data; if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then mem_cnt_next <= 17; elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then mem_cnt_next <= 19; else mem_stage_next <= IDLE; end if; end if; -- READ Deadline 1/2 when 17 => -- Synthesis Guard if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then mem_ready_out <= '1'; -- Memory Flow Control Guard if (mem_valid_out = '1') then mem_data_next.deadline.sec <= mem_read_data; mem_cnt_next <= mem_cnt + 1; end if; end if; -- READ Deadline 2/2 when 18 => -- Synthesis Guard if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then mem_ready_out <= '1'; -- Memory Flow Control Guard if (mem_valid_out = '1') then mem_data_next.deadline.nanosec <= mem_read_data; if check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then mem_cnt_next <= 19; else mem_stage_next <= IDLE; end if; end if; end if; -- READ Result index when 19 => mem_ready_out <= '1'; -- Memory Flow Control Guard if (mem_valid_out = '1') then mem_data_next.res_ind <= mem_read_data; mem_stage_next <= IDLE; end if; when others => null; end case; when GET_NEXT => case (mem_cnt) is -- GET Goal State when 0 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_STATE_OFFSET; mem_read <= '1'; -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- GET Next Addr when 1 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET; mem_read <= '1'; -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- READ Goal State when 2 => mem_ready_out <= '1'; -- Memory Flow Control Guard if (mem_valid_out = '1') then if (mem_read_data(CDR_INT8_WIDTH-1 downto 0) = GoalStatus_package.STATUS_UNKNOWN) then mem_stage_next <= IDLE; mem_abort_read <= '1'; else mem_cnt_next <= mem_cnt + 1; end if; end if; -- READ Next Addr when 3 => mem_ready_out <= '1'; -- Memory Flow Control Guard if (mem_valid_out = '1') then mem_data_next.addr <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); -- No Next Goal if (resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH) = GOAL_MEMORY_MAX_ADDRESS) then -- DONE mem_stage_next <= IDLE; else mem_addr_base_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); mem_stage_next <= GET; if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then mem_cnt_next <= 0; elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then mem_cnt_next <= 1; elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then mem_cnt_next <= 5; elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then mem_cnt_next <= 7; elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then mem_cnt_next <= 9; else -- DONE mem_stage_next <= IDLE; end if; end if; end if; when others => null; end case; when GET_PREV => case (mem_cnt) is -- GET Goal State when 0 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_STATE_OFFSET; mem_read <= '1'; -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- GET Previous Addr when 1 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_PREV_ADDR_OFFSET; mem_read <= '1'; -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- READ Goal State when 2 => mem_ready_out <= '1'; -- Memory Flow Control Guard if (mem_valid_out = '1') then if (mem_read_data(CDR_INT8_WIDTH-1 downto 0) = GoalStatus_package.STATUS_UNKNOWN) then mem_stage_next <= IDLE; mem_abort_read <= '1'; else mem_cnt_next <= mem_cnt + 1; end if; end if; -- READ Previous Addr when 3 => mem_ready_out <= '1'; -- Memory Flow Control Guard if (mem_valid_out = '1') then mem_data_next.addr <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); -- No Previous Goal if (resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH) = GOAL_MEMORY_MAX_ADDRESS) then -- DONE mem_stage_next <= IDLE; else mem_addr_base_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); mem_stage_next <= GET; if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then mem_cnt_next <= 0; elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then mem_cnt_next <= 1; elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then mem_cnt_next <= 5; elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then mem_cnt_next <= 7; elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then mem_cnt_next <= 9; else -- DONE mem_stage_next <= IDLE; end if; end if; end if; when others => null; end case; when UPDATE => case (mem_cnt) is -- SET Goal State when 0 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_STATE_OFFSET; mem_write_data <= (others => '0'); mem_write_data(CDR_INT8_WIDTH-1 downto 0) <= mem_latch_data.state; mem_data_next.state <= mem_latch_data.state; -- Memory Flow Control Guard if (mem_ready_in = '1') then if check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then mem_cnt_next <= 1; elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then mem_cnt_next <= 5; elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then mem_cnt_next <= 7; elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then mem_cnt_next <= 9; else mem_stage_next <= IDLE; end if; end if; -- SET Goal ID 1/4 when 1 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET; mem_write_data <= mem_latch_data.goal_id(0); -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- SET Goal ID 2/4 when 2 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 1; mem_write_data <= mem_latch_data.goal_id(1); -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- SET Goal ID 3/4 when 3 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 2; mem_write_data <= mem_latch_data.goal_id(3); -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- SET Goal ID 4/4 when 4 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 3; mem_write_data <= mem_latch_data.goal_id(3); mem_data_next.goal_id <= mem_latch_data.goal_id; -- Memory Flow Control Guard if (mem_ready_in = '1') then if check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then mem_cnt_next <= 5; elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then mem_cnt_next <= 7; elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then mem_cnt_next <= 9; else mem_stage_next <= IDLE; end if; end if; -- SET Stamp 1/2 when 5 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_STAMP_OFFSET; mem_write_data <= mem_latch_data.stamp.sec; -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- SET Stamp 2/2 when 6 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_STAMP_OFFSET + 1; mem_write_data <= mem_latch_data.stamp.nanosec; mem_data_next.stamp <= mem_latch_data.stamp; -- Memory Flow Control Guard if (mem_ready_in = '1') then if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then mem_cnt_next <= 7; elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then mem_cnt_next <= 9; else mem_stage_next <= IDLE; end if; end if; -- SET Deadline 1/2 when 7 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET; mem_write_data <= mem_latch_data.deadline.sec; -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- SET Deadline 2/2 when 8 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET + 1; mem_write_data <= mem_latch_data.deadline.nanosec; mem_data_next.deadline <= mem_latch_data.deadline; -- Memory Flow Control Guard if (mem_ready_in = '1') then if check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then mem_cnt_next <= 9; else mem_stage_next <= IDLE; end if; end if; -- SET Result Index when 9 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_RESULT_INDEX_OFFSET; mem_write_data <= mem_latch_data.res_ind; mem_data_next.res_ind <= mem_latch_data.res_ind; -- Memory Flow Control Guard if (mem_ready_in = '1') then -- DONE mem_stage_next <= IDLE; end if; when others => null; end case; when GET_RESULT_INDEX => assert (mem_empty_head = GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; case (mem_cnt) is -- GET Next Addr when 0 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_RESULT_INDEX_OFFSET; mem_read <= '1'; -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- READ Next Addr when 1 => mem_ready_out <= '1'; -- Memory Flow Control Guard if (mem_valid_out = '1') then -- Update Empty Result Index empty_head_res_ind_next <= unsigned(mem_read_data); mem_stage_next <= REMOVE; mem_cnt_next <= 0; end if; when others => null; end case; when REMOVE => case (mem_cnt) is -- GET Next Addr when 0 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET; mem_read <= '1'; -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- GET Previous Addr when 1 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_PREV_ADDR_OFFSET; mem_read <= '1'; -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- SET State when 2 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_STATE_OFFSET; mem_write_data(CDR_INT8_WIDTH-1 downto 0) <= GoalStatus_package.STATUS_UNKNOWN; -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- SET Next Addr when 3 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET; mem_write_data <= std_logic_vector(resize(GOAL_MEMORY_MAX_ADDRESS,WORD_WIDTH)); -- Memory Flow Control Guard if (mem_ready_in = '1') then if (mem_empty_tail = GOAL_MEMORY_MAX_ADDRESS) then -- Set New Empty Head/Tail mem_empty_head_next <= mem_addr_base; mem_empty_tail_next <= mem_addr_base; mem_cnt_next <= mem_cnt + 2; -- Skip Next Step else mem_cnt_next <= mem_cnt + 1; end if; end if; -- SET Next Addr (Current Tail) when 4 => assert (mem_empty_tail /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE; mem_valid_in <= '1'; mem_addr <= mem_empty_tail + GMF_NEXT_ADDR_OFFSET; mem_write_data <= std_logic_vector(resize(mem_addr_base,WORD_WIDTH)); -- Memory Flow Control Guard if (mem_ready_in = '1') then -- Set New Empty TAIL mem_empty_tail_next <= mem_addr_base; mem_cnt_next <= mem_cnt + 1; end if; -- READ Next Addr when 5 => mem_ready_out <= '1'; -- Memory Flow Control Guard if (mem_valid_out = '1') then mem_addr_latch_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); -- Set mem_data.addr to previous slot mem_data_next.addr <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); mem_cnt_next <= mem_cnt + 1; end if; -- READ Previous Addr when 6 => mem_ready_out <= '1'; -- Memory Flow Control Guard if (mem_valid_out = '1') then if (mem_addr_latch = GOAL_MEMORY_MAX_ADDRESS) then -- Set New Occupied Tail mem_occupied_tail_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); if (resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH) = GOAL_MEMORY_MAX_ADDRESS) then -- Reset Occupied Head mem_occupied_head_next <= GOAL_MEMORY_MAX_ADDRESS; mem_stage_next <= IDLE; else mem_addr_base_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); mem_cnt_next <= mem_cnt + 2; end if; else mem_addr_base_next <= mem_addr_latch; mem_addr_latch_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH); mem_cnt_next <= mem_cnt + 1; end if; end if; -- SET Previous Addr (Next Slot) when 7 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_PREV_ADDR_OFFSET; mem_write_data <= std_logic_vector(resize(mem_addr_latch,WORD_WIDTH)); -- Memory Flow Control Guard if (mem_ready_in = '1') then if (mem_addr_latch = GOAL_MEMORY_MAX_ADDRESS) then -- Set New Occupied Head mem_occupied_head_next <= mem_addr_base; -- DONE mem_stage_next <= IDLE; else mem_addr_base_next <= mem_addr_latch; mem_addr_latch_next <= mem_addr_base; mem_cnt_next <= mem_cnt + 1; end if; end if; -- SET Next Addr (Previous Slot) when 8 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET; mem_write_data <= std_logic_vector(resize(mem_addr_latch,WORD_WIDTH)); -- Memory Flow Control Guard if (mem_ready_in = '1') then -- DONE mem_stage_next <= IDLE; end if; when others => null; end case; when RESET_MEMORY => case (mem_cnt) is -- SET Goal State when 0 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_STATE_OFFSET; mem_write_data(CDR_INT8_WIDTH-1 downto 0) <= GoalStatus_package.STATUS_UNKNOWN; -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- SET Result Index when 1 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_RESULT_INDEX_OFFSET; mem_write_data <= std_logic_vector(empty_head_res_ind); -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- SET Next Pointer when 2 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET; if (mem_addr_base = MAX_GOAL_ADDRESS) then mem_write_data <= std_logic_vector(resize(GOAL_MEMORY_MAX_ADDRESS,WORD_WIDTH)); else mem_write_data <= std_logic_vector(resize(mem_addr_base + GOAL_FRAME_SIZE,WORD_WIDTH)); end if; -- Memory Flow Control Guard if (mem_ready_in = '1') then mem_cnt_next <= mem_cnt + 1; end if; -- SET Previous Pointer when 3 => mem_valid_in <= '1'; mem_addr <= mem_addr_base + GMF_PREV_ADDR_OFFSET; mem_write_data <= std_logic_vector(resize(mem_addr_latch,WORD_WIDTH)); -- Memory Flow Control Guard if (mem_ready_in = '1') then if (mem_addr_base = MAX_GOAL_ADDRESS) then -- Initialize Empty and Occupied Heads mem_empty_head_next <= FIRST_GOAL_ADDRESS; mem_empty_tail_next <= MAX_GOAL_ADDRESS; mem_occupied_head_next <= GOAL_MEMORY_MAX_ADDRESS; mem_occupied_tail_next <= GOAL_MEMORY_MAX_ADDRESS; empty_head_res_ind_next <= (others => '0'); -- DONE mem_stage_next <= IDLE; else empty_head_res_ind_next <= empty_head_res_ind + 1; mem_addr_latch_next <= mem_addr_base; mem_addr_base_next <= mem_addr_base + GOAL_FRAME_SIZE; mem_cnt_next <= 0; end if; end if; when others => null; end case; end case; end process; sync_prc : process(clk) begin if rising_edge(clk) then if (reset = '1') then stage <= RESET_MEMORY; mem_stage <= RESET_MEMORY; mem_data_r <= ZERO_GOAL_DATA; mem_latch_data <= ZERO_GOAL_DATA; mem_data <= ZERO_GOAL_DATA; rrq_addr_base <= FIRST_RRQ_ADDRESS; rrq_addr_latch <= RRQ_MEMORY_MAX_ADDRESS; mem_addr_base <= FIRST_GOAL_ADDRESS; mem_addr_latch <= GOAL_MEMORY_MAX_ADDRESS; rrq_empty_head <= RRQ_MEMORY_MAX_ADDRESS; rrq_occupied_head <= RRQ_MEMORY_MAX_ADDRESS; mem_occupied_head <= GOAL_MEMORY_MAX_ADDRESS; mem_occupied_tail <= GOAL_MEMORY_MAX_ADDRESS; mem_empty_head <= GOAL_MEMORY_MAX_ADDRESS; mem_empty_tail <= GOAL_MEMORY_MAX_ADDRESS; return_code_latch <= ROS_RET_OK; request_id_latch <= EMPTY_REQUEST_ID; check_time <= ROS_TIME_INFINITE; time_latch <= ROS_TIME_INVALID; goal_handle_latch <= GOAL_HANDLE_UNKNOWN; goal_state_latch <= GoalStatus_package.STATUS_UNKNOWN; g_stamp <= ROS_TIME_INVALID; search_type_c <= SEARCH_NONE; cancel_ret_code <= CancelGoal_package.RR_ERROR_NONE; cnt <= 0; terminal_cnt <= 0; goals_canceling_cnt <= 0; status_list_cnt <= 0; mem_cnt <= 0; trigger_result <= '0'; trigger_status <= '1'; -- Trigger Status Update immediately g_accept <= '0'; empty_head_res_ind <= (others => '0'); else stage <= stage_next; mem_stage <= mem_stage_next; mem_data_r <= mem_data_r_next; mem_latch_data <= mem_latch_data_next; mem_data <= mem_data_next; rrq_addr_base <= rrq_addr_base_next; rrq_addr_latch <= rrq_addr_latch_next; mem_addr_base <= mem_addr_base_next; mem_addr_latch <= mem_addr_latch_next; rrq_empty_head <= rrq_empty_head_next; rrq_occupied_head <= rrq_occupied_head_next; mem_occupied_head <= mem_occupied_head_next; mem_occupied_tail <= mem_occupied_tail_next; mem_empty_head <= mem_empty_head_next; mem_empty_tail <= mem_empty_tail_next; return_code_latch <= return_code_latch_next; request_id_latch <= request_id_latch_next; check_time <= check_time_next; time_latch <= time_latch_next; goal_handle_latch <= goal_handle_latch_next; goal_state_latch <= goal_state_latch_next; g_stamp <= g_stamp_next; search_type_c <= search_type_c_next; cancel_ret_code <= cancel_ret_code_next; cnt <= cnt_next; terminal_cnt <= terminal_cnt_next; goals_canceling_cnt <= goals_canceling_cnt_next; status_list_cnt <= status_list_cnt_next; mem_cnt <= mem_cnt_next; trigger_result <= trigger_result_next; trigger_status <= trigger_status_next; g_accept <= g_accept_next; empty_head_res_ind <= empty_head_res_ind_next; end if; end if; end process; end architecture;