greek64/rtps-fpga
2
1
Fork 0
Code Issues Pull Requests Releases Wiki Activity
d537cd7460
rtps-fpga/src/ros2/ros_action_server.vhd

3483 lines
183 KiB
VHDL
Raw Blame History

-- 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;
Reference in New Issue View Git Blame Copy Permalink