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

3414 lines
173 KiB
Plaintext
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;
entity ros_action_server is
generic (
TIMEOUT_DURATION : ROS_DURATION_TYPE;
MAX_GOALS : natural;
MAX_RESULT_REQUESTS : natural
);
port (
-- SYSTEM
clk : in std_logic;
reset : in std_logic;
time : in ROS_TIME_TYPE;
-- *GOAL SERVICE*
start_g : out std_logic;
ack_g : in std_logic;
opcode_g : out ROS_SERVICE_OPCODE_TYPE;
service_info_g : in SERVICE_INFO_TYPE;
request_id_g : out REQUEST_ID_TYPE;
data_available_g : in std_logic;
taken_g : in std_logic;
done_g : in std_logic;
return_code_g : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
-- REQUEST
goal_id_g : in std_logic_vector(UUID_WIDTH-1 downto 0);
-- RESPONSE
accepted_g : out std_logic;
stamp_g : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
-- *RESULT SERVICE*
start_r : out std_logic;
ack_r : in std_logic;
opcode_r : out ROS_SERVICE_OPCODE_TYPE;
service_info_r : in SERVICE_INFO_TYPE;
request_id_r : out REQUEST_ID_TYPE;
data_available_r : in std_logic;
taken_r : in std_logic;
done_r : in std_logic;
return_code_r : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
result_index : out std_logic_vector(WORD_WIDTH-1 downto 0);
result_sel : out std_logic;
result_sel_ack : in std_logic;
-- REQUEST
goal_id_r : in std_logic_vector(UUID_WIDTH-1 downto 0);
-- RESPONSE
status_r : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
-- *CANCEL SERVICE*
start_c : out std_logic;
ack_c : in std_logic;
opcode_c : out ROS_SERVICE_OPCODE_TYPE;
service_info_c : in SERVICE_INFO_TYPE;
request_id_c : out REQUEST_ID_TYPE;
data_available_c : in std_logic;
taken_c : in std_logic;
done_c : in std_logic;
return_code_c : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
-- REQUEST
goal_info_goal_id_c : in std_logic_vector(UUID_WIDTH-1 downto 0);
goal_info_stamp_c : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
-- RESPONSE
cancel_return_code_c : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
goals_canceling_len_c : out std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0);
goals_canceling_addr_c : out std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0);
goals_canceling_ready_c : in std_logic;
goals_canceling_ren_c : out std_logic;
goals_canceling_wen_c : out std_logic;
goals_canceling_valid_c : in std_logic;
goals_canceling_ack_c : out std_logic;
goals_canceling_goal_id_r_c : in std_logic_vector(UUID_WIDTH-1 downto 0);
goals_canceling_goal_id_w_c : out std_logic_vector(UUID_WIDTH-1 downto 0);
goals_canceling_stamp_r_c : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
goals_canceling_stamp_w_c : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
-- *FEEDBACK TOPIC*
start_fb : out std_logic;
opcode_fb : out ROS_TOPIC_OPCODE_TYPE;
ack_fb : in std_logic;
done_fb : in std_logic;
return_code_fb : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
goal_id_fb : out std_logic_vector(UUID_WIDTH-1 downto 0);
-- *STATUS TOPIC*
start_s : out std_logic;
opcode_s : out ROS_TOPIC_OPCODE_TYPE;
ack_s : in std_logic;
done_s : in std_logic;
return_code_s : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
status_list_len_s : out std_logic_vector(GoalStatusArray_package.STATUS_LIST_ADDR_WIDTH-1 downto 0);
status_list_addr_s : out std_logic_vector(GoalStatusArray_package.STATUS_LIST_ADDR_WIDTH-1 downto 0);
status_list_ready_s : in std_logic;
status_list_ren_s : out std_logic;
status_list_wen_s : out std_logic;
status_list_valid_s : in std_logic;
status_list_ack_s : out std_logic;
status_list_goal_info_goal_id_r_s : in std_logic_vector(UUID_WIDTH-1 downto 0);
status_list_goal_info_goal_id_w_s : out std_logic_vector(UUID_WIDTH-1 downto 0);
status_list_goal_info_stamp_r_s : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
status_list_goal_info_stamp_w_s : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
status_list_status_r_s : in std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
status_list_status_w_s : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
-- *USER*
start : in std_logic;
opcode : in ROS_ACTION_OPCODE_TYPE;
ack : out std_logic;
done : out std_logic;
return_code : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
goal_handle_in : in std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
goal_handle_out : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
goal_state_in : in std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
goal_state_out : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
goal_id : out std_logic_vector(UUID_WIDTH-1 downto 0);
goal_result_index : out std_logic_vector(WORD_WIDTH-1 downto 0);
goal_stamp : out ROS_TIME_TYPE;
-- GOAL
new_goal_request : out std_logic;
new_goal_handle : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
new_goal_result_index : out std_logic_vector(WORD_WIDTH-1 downto 0);
new_goal_accepted : in std_logic;
new_goal_response : in std_logic;
-- CANCEL
cancel_request : out std_logic;
cancel_request_handle : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
cancel_accepted : in std_logic;
cancel_response : in std_logic
);
end entity;
architecture arch of ros_action_server is
--*****CONSTANT DECLARATION*****
-- *GOAL MEMORY*
-- 4-Byte Word Size of a Goal Entry in Memory
function gen_goal_frame_size(TIMEOUT_DURATION : ROS_DURATION_TYPE) return natural is
begin
if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then
return 12;
else
return 10;
end if;
end function;
constant GOAL_FRAME_SIZE : natural := gen_goal_frame_size(TIMEOUT_DURATION);
-- Goal Memory Size in 4-Byte Words
constant GOAL_MEMORY_SIZE : natural := MAX_GOALS * GOAL_FRAME_SIZE;
-- Goal Memory Address Width
constant GOAL_MEMORY_ADDR_WIDTH : natural := log2c(GOAL_MEMORY_SIZE);
-- Highest Goal Memory Address
constant GOAL_MEMORY_MAX_ADDRESS : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0) := to_unsigned(GOAL_MEMORY_SIZE-1, GOAL_MEMORY_ADDR_WIDTH);
-- Highest Goal Frame Address
constant MAX_GOAL_ADDRESS : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0) := GOAL_MEMORY_MAX_ADDRESS - GOAL_FRAME_SIZE + 1;
-- Address pointing to the beginning of the first Goal Data Frame
constant FIRST_GOAL_ADDRESS : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0');
-- *RESULT REQUEST MEMORY*
-- 4-Byte Word Size of a Result Request Entry in Memory
constant RRQ_FRAME_SIZE : natural := 9;
-- Result Request Memory Size in 4-Byte Words
constant RRQ_MEMORY_SIZE : natural := MAX_RESULT_REQUESTS * RRQ_FRAME_SIZE;
-- Result Request Memory Address Width
constant RRQ_MEMORY_ADDR_WIDTH : natural := log2c(RRQ_MEMORY_SIZE);
-- Highest Result Request Memory Address
constant RRQ_MEMORY_MAX_ADDRESS : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0) := to_unsigned(RRQ_MEMORY_SIZE-1, RRQ_MEMORY_ADDR_WIDTH);
-- Highest Result Request Frame Address
constant MAX_RRQ_ADDRESS : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0) := RRQ_MEMORY_MAX_ADDRESS - RRQ_FRAME_SIZE + 1;
-- Address pointing to the beginning of the first Result Request Data Frame
constant FIRST_RRQ_ADDRESS : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0');
-- *GOAL MEMORY FRAME FIELD OFFSETS*
-- 4-Byte Word Offsets to Beginning of Respective Fields in the Goal Memory Frame
constant GMF_STATE_OFFSET : natural := 0;
constant GMF_GOAL_ID_OFFSET : natural := 1;
constant GMF_STAMP_OFFSET : natural := 5;
constant GMF_DEADLINE_OFFSET : natural := 7;
function gen_gmf_result_index_offset(TIMEOUT_DURATION : ROS_DURATION_TYPE) return natural is
variable ret : natural := 0;
begin
if (TIMEOUT_DURATION = ROS_DURATION_INFINITE) then
ret := GMF_DEADLINE_OFFSET + 2;
else
ret := GMF_DEADLINE_OFFSET;
end if;
return ret;
end function;
constant GMF_RESULT_INDEX_OFFSET : natural := gen_gmf_result_index_offset(TIMEOUT_DURATION);
constant GMF_NEXT_ADDR_OFFSET : natural := GMF_RESULT_INDEX_OFFSET + 1;
constant GMF_PREV_ADDR_OFFSET : natural := GMF_NEXT_ADDR_OFFSET + 1;
-- *RESULT REQUEST MEMORY FRAME FIELD OFFSETS*
-- 4-Byte Word Offsets to Beginning of Respective Fields in the Result Request Memory Frame
constant RMF_GOAL_HANDLE_OFFSET : natural := 0;
constant RMF_REQUEST_ID_OFFSET : natural := 1;
constant RMF_NEXT_ADDR_OFFSET : natural := 7;
constant RMF_PREV_ADDR_OFFSET : natural := 8;
-- *GOAL MEMORY FRAME FIELD FLAGS*
-- Flags mapping to the respective Goal Memory Frame Fields
constant GMF_FLAG_WIDTH : natural := 7;
constant GMF_STATE_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (0 => '1', others => '0');
constant GMF_GOAL_ID_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (1 => '1', others => '0');
constant GMF_STAMP_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (2 => '1', others => '0');
constant GMF_DEADLINE_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (3 => '1', others => '0');
constant GMF_RESULT_INDEX_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (4 => '1', others => '0');
constant GMF_NEXT_ADDR_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (5 => '1', others => '0');
constant GMF_PREV_ADDR_FLAG : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (6 => '1', others => '0');
--*****TYPE DECLARATION*****
-- *Cancel Process Search Types*
-- SEARCH_GOAL_ID Search goal list for a specific goal id
-- SEARCH_STAMP Search goal list for first goal that was accepted at or before a provided timestamp
-- SEARCH_NONE No search criteria. All goals are valid candidates (Applies from current list position)
type SEARCH_TYPE is (SEARCH_GOAL_ID, SEARCH_STAMP, SEARCH_NONE);
-- *Instance Memory Opcodes*
-- OPCODE DESCRIPTION
-- INSERT Insert goal into memory
-- GET Get data of goal pointed by "GOAL_DATA_TYPE.addr" according to "GOAL_DATA_TYPE.field_flags".
-- Already fetched data of the goal is not modified.
-- GET_NEXT Get goal data of next goal (from the goal pointed by "GOAL_DATA_TYPE.addr") according to "GOAL_DATA_TYPE.field_flags".
-- Set "GOAL_DATA_TYPE.addr" to address of goal or GOAL_MEMORY_MAX_ADDRESS if no other goal in Memory.
-- GET_PREV Get goal data of previous goal (from the goal pointed by "GOAL_DATA_TYPE.addr") according to "GOAL_DATA_TYPE.field_flags".
-- Set "GOAL_DATA_TYPE.addr" to address of goal or GOAL_MEMORY_MAX_ADDRESS if no other goal in Memory.
-- UPDATE Update goal data pointed by "GOAL_DATA_TYPE.addr" according to "GOAL_DATA_TYPE.field_flags"
-- REMOVE Remove goal pointed by "GOAL_DATA_TYPE.addr"
type MEM_OPCODE_TYPE is (NOP, INSERT, GET, GET_NEXT, GET_PREV, UPDATE, REMOVE);
type STAGE_TYPE is (IDLE,WAIT_FOR_DATA,SEARCH_ID,CHECK_STATE,STORE_RRQ,SEND_RESPONSE,WAIT_FOR_RET,TRANSITION_STATE,GET_DATA,CHECK_REQUESTS,GET_REQUEST_ID,REMOVE_REQUEST,WAIT_FOR_MEM,RETURN_USER,CHECK_TIMEOUT,REMOVE_OLDEST,PUBLISH_FEEDBACK,RESET_MEMORY);
type STATUS_STAGE_TYPE is (IDLE,GET_FIRST,GET_NEXT,WAIT_FOR_DATA,PUSH_STATUS,PUBLISH,WAIT_FOR_PUBLISHER);
type GOAL_STAGE_TYPE is (IDLE,WAIT_FOR_DATA,WAIT_FOR_USER,ADD_GOAL,SEND_RESPONSE,WAIT_FOR_RET);
type CANCEL_STAGE_TYPE is (IDLE,WAIT_FOR_DATA,GET,GET_NEXT,CHECK,CHECK_GOAL_ID,CHECK_STAMP,CHECK_STATE,WAIT_FOR_USER,CANCEL_GOAL,ADD_CANCEL,SEND_RESPONSE,WAIT_FOR_RET);
type MEM_STAGE_TYPE is (IDLE,INSERT,GET,GET_NEXT,GET_PREV,UPDATE,REMOVE,RESET_MEMORY);
type GOAL_DATA_TYPE is record
addr : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0);
goal_id : GUID_TYPE;
stamp : ROS_TIME_TYPE;
deadline : ROS_DURATION_TYPE;
res_ind : std_logic_vector(WORD_WIDTH-1 downto 0);
state : std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
field_flags : std_logic_vector(0 to GMF_FLAG_WIDTH-1);
end record;
constant ZERO_GOAL_DATA : GOAL_DATA_TYPE := (
addr => (others => '0'),
goal_id => GUID_UNKNOWN,
stamp => ROS_TIME_ZERO,
deadline => ROS_DURATION_INFINITE,
res_ind => (others => '0'),
state => GoalStatus_package.STATUS_UNKNOWN,
field_flags => (others => '0')
);
--*****SIGNAL DECLARATION*****
-- *GOAL MEMORY CONNECTION SIGNALS*
signal mem_addr : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0);
signal mem_read : std_logic;
signal mem_read_data, mem_write_data : std_logic_vector(WORD_WIDTH-1 downto 0);
signal mem_ready_in, mem_valid_in : std_logic;
signal mem_ready_out, mem_valid_out : std_logic;
signal mem_abort_read : std_logic;
-- *RESULT REQUEST MEMORY CONNECTION SIGNALS*
signal rrq_addr : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0);
signal rrq_read : std_logic;
signal rrq_read_data, rrq_write_data : std_logic_vector(WORD_WIDTH-1 downto 0);
signal rrq_ready_in, rrq_valid_in : std_logic;
signal rrq_ready_out, rrq_valid_out : std_logic;
signal rrq_abort_read : std_logic;
-- *MAIN PROCESS*
-- FSM state
signal stage, stage_next : STAGE_TYPE;
-- Request memory operation
signal mem_start_r : std_logic;
-- Opcode of goal memory operation (valid only if mem_start_r is high)
signal mem_opcode_r : MEM_OPCODE_TYPE;
-- Signal used to pass data to Goal Memory Process
signal mem_r : GOAL_DATA_TYPE;
-- Goal memory data latch
signal mem_data_r, mem_data_r_next : GOAL_DATA_TYPE;
-- Head of occupied result requests list
signal rrq_occupied_head, rrq_occupied_head_next : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0);
-- Head of empty result requests list
signal rrq_empty_head, rrq_empty_head_next : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0);
-- General purpose counter
signal cnt, cnt_next : natural range 0 to 11;
-- Pointer to current relevant Result Request Memory Frame
signal rrq_addr_base, rrq_addr_base_next : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0);
-- Result Request Memory Frame pointer latch
signal rrq_addr_latch, rrq_addr_latch_next : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0);
-- Count of stored goals that are in a terminal state
signal terminal_cnt, terminal_cnt_next : natural range 0 to MAX_GOALS;
-- User return code latch
signal return_code_latch, return_code_latch_next : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
-- Toggle latch used to trigger stored result request search when a goal is updated to a terminal state
signal trigger_result, trigger_result_next : std_logic;
-- Signal used to signal goal status change (Used by status process to generate new goal status list)
signal trigger_status_r : std_logic;
-- Result service Request ID latch
signal request_id_latch, request_id_latch_next : REQUEST_ID_TYPE;
-- Time of next goal expiration deadline
signal check_time, check_time_next : ROS_TIME_TYPE;
-- Temporal time latch (used in time addition calculation)
signal time_latch, time_latch_next : ROS_TIME_TYPE;
-- Signals that the main FSM has entered the atomic operation range (Used mutex lock between main and cancel FSMs)
signal atomic_op_r : std_logic;
-- Goal handle latch
signal goal_handle_latch, goal_handle_latch_next : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
-- Goal state latch
signal goal_state_latch, goal_state_latch_next :std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
-- *GOAL PROCESS*
-- Goal Process FSM state
signal goal_stage, goal_stage_next : GOAL_STAGE_TYPE;
-- Request memory operation
signal mem_start_g : std_logic;
-- Opcode of memory operation (valid only if mem_start_g is high)
signal mem_opcode_g : MEM_OPCODE_TYPE;
-- Goal timestamp latch
signal g_stamp, g_stamp_next : ROS_TIME_TYPE;
-- Goal accept latch
signal g_accept, g_accept_next : std_logic;
-- Signal used to signal goal status change (Used by status process to generate new goal status list)
signal trigger_status_g : std_logic;
-- *CANCEL PROCESS*
-- Cancel process FSM state
signal cancel_stage, cancel_stage_next : CANCEL_STAGE_TYPE;
-- Request memory operation
signal mem_start_c : std_logic;
-- Opcode of memory operation (valid only if mem_start_c is high)
signal mem_opcode_c : MEM_OPCODE_TYPE;
-- Signal used to pass data to Goal Memory Process
signal mem_c : GOAL_DATA_TYPE;
-- General purpose counter
signal c_cnt, c_cnt_next : natural range 0 to 3;
-- Goal memory data latch
signal mem_data_c, mem_data_c_next : GOAL_DATA_TYPE;
-- Signals the kind of search the cancel FSM is doing (see SEARCH_TYPE definition)
signal search_type_c, search_type_c_next : SEARCH_TYPE;
-- Index of goals_canceling sequence
signal goals_canceling_cnt, goals_canceling_cnt_next : natural range 0 to CancelGoal_package.RR_GOALS_CANCELING_MAX_DEPTH-1;
-- Cancel response return code latch
signal cancel_ret_code, cancel_ret_code_next : std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
-- Signal used to signal goal status change (Used by status process to generate new goal status list)
signal trigger_status_c : std_logic;
-- Signals that the cancel FSM has entered the atomic operation range (Used mutex lock between main and cancel FSMs)
signal atomic_op_c: std_logic;
-- *STATUS PROCESS*
-- Status process FSM
signal status_stage, status_stage_next : STATUS_STAGE_TYPE;
-- Toggle latch that latches status update requests from other processes
signal trigger_status, trigger_status_next : std_logic;
-- Index of status list sequence
signal status_list_cnt, status_list_cnt_next : natural range 0 to MAX_GOALS-1;
-- Request memory operation
signal mem_start_s : std_logic;
-- Opcode of memory operation (valid only if mem_start_s is high)
signal mem_opcode_s : MEM_OPCODE_TYPE;
-- Signal used to pass data to Goal Memory Process
signal mem_s : GOAL_DATA_TYPE;
-- Goal memory data latch
signal mem_data_s, mem_data_s_next : GOAL_DATA_TYPE;
-- Test signal used for testbench synchronisation
signal status_idle_sig : std_logic;
-- *GOAL MEMORY PROCESS*
-- Memory process FSM state
signal mem_stage, mem_stage_next : MEM_STAGE_TYPE;
-- Acknowledge memory request from respective process
signal mem_ack_r, mem_ack_s, mem_ack_c, mem_ack_g : std_logic;
-- Head of occupied goal list
signal mem_occupied_head, mem_occupied_head_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0);
-- Tail of occupied goal list
signal mem_occupied_tail, mem_occupied_tail_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0);
-- Head of empty goal list
signal mem_empty_head, mem_empty_head_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0);
-- Tail of empty goal list
signal mem_empty_tail, mem_empty_tail_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0);
-- General purpose counter
signal mem_cnt, mem_cnt_next : natural range 0 to 19;
-- Pointer to currently relevant goal memory frame
signal mem_addr_base, mem_addr_base_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0);
-- Goal memory frame pointer latch
signal mem_addr_latch, mem_addr_latch_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0);
-- Result index of current empty head goal
signal empty_head_res_ind, empty_head_res_ind_next : unsigned(WORD_WIDTH-1 downto 0);
-- Latch for data from processes
signal mem_latch_data, mem_latch_data_next : GOAL_DATA_TYPE;
-- Latch for goal memory data
signal mem_data, mem_data_next : GOAL_DATA_TYPE;
-- Signals end of memory operation (And that the memory process is in the IDLE state)
signal mem_done : std_logic;
-- *FUNCTION DECLARATION*
-- Returns true if the state is terminal
function is_terminal (state : std_logic_vector) return boolean is
begin
assert (state'length = CDR_INT8_WIDTH) severity FAILURE;
case (state) is
when GoalStatus_package.STATUS_SUCCEEDED =>
return TRUE;
when GoalStatus_package.STATUS_CANCELED =>
return TRUE;
when GoalStatus_package.STATUS_ABORTED =>
return TRUE;
when others =>
return FALSE;
end case;
end function;
begin
goal_mem_ctrl_inst : entity work.mem_ctrl(arch)
generic map (
ADDR_WIDTH => GOAL_MEMORY_ADDR_WIDTH,
DATA_WIDTH => WORD_WIDTH,
MEMORY_DEPTH => GOAL_MEMORY_SIZE,
MAX_BURST_LENGTH => GOAL_FRAME_SIZE
)
port map (
clk => clk,
reset => reset or mem_abort_read,
addr => std_logic_vector(mem_addr),
read => mem_read,
ready_in => mem_ready_in,
valid_in => mem_valid_in,
data_in => mem_write_data,
ready_out => mem_ready_out,
valid_out => mem_valid_out,
data_out => mem_read_data
);
rrq_mem_ctrl_inst : entity work.mem_ctrl(arch)
generic map (
ADDR_WIDTH => RRQ_MEMORY_ADDR_WIDTH,
DATA_WIDTH => WORD_WIDTH,
MEMORY_DEPTH => RRQ_MEMORY_SIZE,
MAX_BURST_LENGTH => RRQ_FRAME_SIZE
)
port map (
clk => clk,
reset => reset or rrq_abort_read,
addr => std_logic_vector(rrq_addr),
read => rrq_read,
ready_in => rrq_ready_in,
valid_in => rrq_valid_in,
data_in => rrq_write_data,
ready_out => rrq_ready_out,
valid_out => rrq_valid_out,
data_out => rrq_read_data
);
result_index <= mem_data_r.res_ind;
status_r <= mem_data_r.state;
request_id_r <= request_id_latch;
goal_id_fb <= std_logic_vector(to_unsigned(mem_data_r.goal_id));
goal_id <= std_logic_vector(to_unsigned(mem_data_r.goal_id));
goal_stamp <= mem_data_r.stamp;
goal_state_out <= mem_data_r.state;
goal_result_index <= mem_data_r.res_ind;
request_id_g <= service_info_g.request_id;
accepted_g <= g_accept;
stamp_g <= std_logic_vector(to_unsigned(g_stamp));
request_id_c <= service_info_c.request_id;
cancel_return_code_c <= cancel_ret_code;
goals_canceling_len_c <= std_logic_vector(to_unsigned(goals_canceling_cnt,goals_canceling_len_c'length));
status_list_len_s <= std_logic_vector(to_unsigned(status_list_cnt,status_list_len_s'length));
-- *Main State Machine*
-- STATE DESCRIPTION
-- IDLE Idle state
-- WAIT_FOR_DATA Wait for result service request
-- SEARCH_ID Search for goal with result request goal id
-- CHECK_STATE Check if found goal is in terminal state
-- STORE_RRQ Store result request in memory
-- SEND_RESPONSE Send result service response
-- WAIT_FOR_RET Wait for result service response
-- TRANSITION_STATE Update goal state
-- GET_DATA Get goal data required for result response
-- CHECK_REQUESTS Search if updated goal is in stored result requests
-- GET_REQUEST_ID Get data from stored result request
-- REMOVE_REQUEST Remove stored result request
-- WAIT_FOR_MEM Wait for goal data from memory
-- RETURN_USER User return
-- CHECK_TIMEOUT Check and remove expired goals
-- REMOVE_OLDEST Remove oldest goal in terminal state
-- PUBLISH_FEEDBACK Publish feedback
-- RESET_MEMORY Reset result request memory to empty state
main_prc : process(all)
begin
-- DEFAULT
stage_next <= stage;
cnt_next <= cnt;
mem_data_r_next <= mem_data_r;
terminal_cnt_next <= terminal_cnt;
return_code_latch_next <= return_code_latch;
trigger_result_next <= trigger_result;
rrq_addr_base_next <= rrq_addr_base;
rrq_addr_latch_next <= rrq_addr_latch;
rrq_empty_head_next <= rrq_empty_head;
rrq_occupied_head_next <= rrq_occupied_head;
request_id_latch_next <= request_id_latch;
time_latch_next <= time_latch;
check_time_next <= check_time;
goal_handle_latch_next <= goal_handle_latch;
goal_state_latch_next <= goal_state_latch;
-- DEFAULT Unregistered
ack <= '0';
done <= '0';
start_fb <= '0';
rrq_abort_read <= '0';
result_sel <= '0';
atomic_op_r <= '0';
trigger_status_r <= '0';
mem_start_r <= '0';
mem_opcode_r <= NOP;
start_r <= '0';
opcode_r <= NOP;
opcode_fb <= NOP;
mem_r <= ZERO_GOAL_DATA;
return_code <= ROS_RET_OK;
goal_handle_out <= GOAL_HANDLE_UNKNOWN;
case (stage) is
when IDLE =>
if (start = '1') then
case (opcode) is
when GET_GOAL =>
mem_start_r <= '1';
mem_opcode_r <= GET;
mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG;
mem_r.addr <= unsigned(goal_handle_in);
if (mem_ack_r = '1') then
ack <= '1';
stage_next <= WAIT_FOR_MEM;
return_code_latch_next <= ROS_RET_OK;
end if;
when GET_LAST_GOAL =>
-- No Goals Available
if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then
mem_data_r_next.addr <= GOAL_MEMORY_MAX_ADDRESS;
ack <= '1';
return_code_latch_next <= ROS_RET_OK;
stage_next <= RETURN_USER;
else
mem_start_r <= '1';
mem_opcode_r <= GET;
mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG;
mem_r.addr <= mem_occupied_head;
if (mem_ack_r = '1') then
ack <= '1';
stage_next <= WAIT_FOR_MEM;
return_code_latch_next <= ROS_RET_OK;
end if;
end if;
when GET_PREVIOUS_GOAL =>
mem_start_r <= '1';
mem_opcode_r <= GET_NEXT;
mem_r.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG;
mem_r.addr <= unsigned(goal_handle_in);
if (mem_ack_r = '1') then
ack <= '1';
stage_next <= WAIT_FOR_MEM;
return_code_latch_next <= ROS_RET_OK;
end if;
when UPDATE_GOAL_STATE =>
-- Atomic Operation Guard
if (atomic_op_c = '1' and unsigned(goal_handle_in) = mem_data_c.addr) then
-- NOTE: In order to prevent a user side Deadlock, we return with INVALID_ARGUMENT if the cancel FSM is also waiting for a user input in the atomic operation zone.
if (cancel_stage = WAIT_FOR_USER) then
ack <= '1';
stage_next <= RETURN_USER;
return_code_latch_next <= ROS_RET_INVALID_ARGUMENT;
end if;
else
mem_start_r <= '1';
mem_opcode_r <= GET;
mem_r.field_flags <= GMF_STATE_FLAG;
mem_r.addr <= unsigned(goal_handle_in);
if (mem_ack_r = '1') then
-- Latch Input Data
goal_handle_latch_next <= goal_handle_in;
goal_state_latch_next <= goal_state_in;
ack <= '1';
stage_next <= TRANSITION_STATE;
cnt_next <= 0;
end if;
end if;
when PUBLISH_FEEDBACK =>
mem_start_r <= '1';
mem_opcode_r <= GET_NEXT;
mem_r.field_flags <= GMF_GOAL_ID_FLAG;
mem_r.addr <= unsigned(goal_handle_in);
if (mem_ack_r = '1') then
ack <= '1';
stage_next <= PUBLISH_FEEDBACK;
cnt_next <= 0;
end if;
when others =>
null;
end case;
elsif (data_available_r = '1' and rrq_empty_head /= RRQ_MEMORY_MAX_ADDRESS) then
start_r <= '1';
opcode_r <= TAKE_REQUEST;
if (ack_r = '1') then
stage_next <= WAIT_FOR_DATA;
end if;
elsif (TIMEOUT_DURATION /= ROS_TIME_INFINITE and time >= check_time) then
assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE;
-- Reset
check_time_next <= ROS_TIME_INFINITE;
mem_data_r_next.addr <= mem_occupied_head;
stage_next <= CHECK_TIMEOUT;
cnt_next <= 0;
elsif (TIMEOUT_DURATION = ROS_TIME_INFINITE and mem_empty_head = GOAL_MEMORY_MAX_ADDRESS and terminal_cnt /= 0) then
assert (mem_occupied_tail /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE;
mem_data_r_next.addr <= mem_occupied_tail;
stage_next <= REMOVE_OLDEST;
cnt_next <= 0;
end if;
when WAIT_FOR_DATA =>
if (done_r = '1') then
case (return_code_r) is
when ROS_RET_OK =>
if (taken_r = '1') then
-- Latch Request ID
request_id_latch_next <= service_info_r.request_id;
-- No Goals Available
if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then
mem_data_r_next.res_ind <= std_logic_vector(to_unsigned(MAX_GOALS, WORD_WIDTH));
mem_data_r_next.state <= GoalStatus_package.STATUS_UNKNOWN;
stage_next <= SEND_RESPONSE;
else
stage_next <= SEARCH_ID;
cnt_next <= 0;
end if;
else
stage_next <= IDLE;
end if;
when others =>
-- TODO: Propagate Error?
stage_next <= IDLE;
end case;
end if;
when SEARCH_ID =>
case (cnt) is
-- GET FIRST
when 0 =>
assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE;
mem_start_r <= '1';
mem_opcode_r <= GET;
mem_r.field_flags <= GMF_GOAL_ID_FLAG;
mem_r.addr <= mem_occupied_head;
if (mem_ack_r = '1') then
cnt_next <= cnt + 2;
end if;
-- GET NEXT
when 1 =>
mem_start_r <= '1';
mem_opcode_r <= GET_NEXT;
mem_r.field_flags <= GMF_GOAL_ID_FLAG;
mem_r.addr <= mem_data_r.addr;
if (mem_ack_r = '1') then
cnt_next <= cnt + 1;
end if;
-- CHECK
when 2 =>
-- Wait for Memory
if (mem_done = '1') then
assert check_mask(mem_data.field_flags, GMF_GOAL_ID_FLAG) severity FAILURE;
-- No more Goals
if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then
-- Goal not found
mem_data_r_next.res_ind <= std_logic_vector(to_unsigned(MAX_GOALS, WORD_WIDTH));
mem_data_r_next.state <= GoalStatus_package.STATUS_UNKNOWN;
stage_next <= SEND_RESPONSE;
else
mem_data_r_next <= mem_data;
cnt_next <= cnt + 1;
end if;
end if;
-- CHECK GOAL_ID 1/4
when 3 =>
if (mem_data_r.goal_id(0) = to_GUID(goal_id_r)(0)) then
cnt_next <= cnt + 1;
else
cnt_next <= 1; -- GET_NEXT
end if;
-- CHECK GOAL_ID 2/4
when 4 =>
if (mem_data_r.goal_id(1) = to_GUID(goal_id_r)(1)) then
cnt_next <= cnt + 1;
else
cnt_next <= 1; -- GET_NEXT
end if;
-- CHECK GOAL_ID 3/4
when 5 =>
if (mem_data_r.goal_id(2) = to_GUID(goal_id_r)(2)) then
cnt_next <= cnt + 1;
else
cnt_next <= 1; -- GET_NEXT
end if;
-- CHECK GOAL_ID 4/4
when 6 =>
if (mem_data_r.goal_id(3) = to_GUID(goal_id_r)(3)) then
-- Found Goal
stage_next <= CHECK_STATE;
cnt_next <= 0;
else
cnt_next <= 1; -- GET_NEXT
end if;
when others =>
null;
end case;
when CHECK_STATE =>
case (cnt) is
-- GET STATE
when 0 =>
mem_start_r <= '1';
mem_opcode_r <= GET;
mem_r.field_flags <= GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG;
mem_r.addr <= mem_data_r.addr;
if (mem_ack_r = '1') then
cnt_next <= cnt + 1;
end if;
-- Check State
when 1 =>
-- Wait for Memory
if (mem_done = '1') then
assert (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) severity FAILURE;
assert check_mask(mem_data.field_flags, GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG) severity FAILURE;
mem_data_r_next <= mem_data;
if (is_terminal(mem_data.state)) then
if (mem_data.state /= GoalStatus_package.STATUS_SUCCEEDED) then
-- NOTE: We are setting result index to MAX_GOALS to effectively "zero" the result.
-- This prevents sending the result of previous succeeded goals, if the current
-- goal is not succeeded.
mem_data_r_next.res_ind <= std_logic_vector(to_unsigned(MAX_GOALS, WORD_WIDTH));
end if;
stage_next <= SEND_RESPONSE;
else
stage_next <= STORE_RRQ;
cnt_next <= 0;
end if;
end if;
when others =>
null;
end case;
when STORE_RRQ =>
assert (rrq_empty_head /= RRQ_MEMORY_MAX_ADDRESS) severity FAILURE;
case (cnt) is
-- GET Next Addr
when 0 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_empty_head + RMF_NEXT_ADDR_OFFSET;
rrq_read <= '1';
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
-- SET Goal Handle
when 1 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_empty_head + RMF_GOAL_HANDLE_OFFSET;
rrq_write_data <= std_logic_vector(resize(mem_data_r.addr,WORD_WIDTH));
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
-- SET Request ID 1/6
when 2 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET;
rrq_write_data <= service_info_c.request_id.writer_guid(0);
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
-- SET Request ID 2/6
when 3 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 1;
rrq_write_data <= service_info_c.request_id.writer_guid(1);
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
-- SET Request ID 3/6
when 4 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 2;
rrq_write_data <= service_info_c.request_id.writer_guid(2);
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
-- SET Request ID 4/6
when 5 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 3;
rrq_write_data <= service_info_c.request_id.writer_guid(3);
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
-- SET Request ID 5/6
when 6 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 4;
rrq_write_data <= std_logic_vector(service_info_c.request_id.sequence_number(0));
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
-- SET Request ID 6/6
when 7 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 5;
rrq_write_data <= std_logic_vector(service_info_c.request_id.sequence_number(1));
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
if (rrq_occupied_head = RRQ_MEMORY_MAX_ADDRESS) then
cnt_next <= cnt + 2;
else
cnt_next <= cnt + 1;
end if;
end if;
-- SET Prev Addr (Old Occupied Head)
when 8 =>
assert (rrq_occupied_head /= RRQ_MEMORY_MAX_ADDRESS) severity FAILURE;
rrq_valid_in <= '1';
rrq_addr <= rrq_occupied_head + RMF_PREV_ADDR_OFFSET;
rrq_write_data <= std_logic_vector(resize(rrq_empty_head,WORD_WIDTH));
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
-- SET Next Addr
when 9 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_empty_head + RMF_NEXT_ADDR_OFFSET;
rrq_write_data <= std_logic_vector(resize(rrq_occupied_head,WORD_WIDTH));
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
-- Set New Occupied Head
rrq_occupied_head_next <= rrq_empty_head;
cnt_next <= cnt + 1;
end if;
-- SET Prev Addr
when 10 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_empty_head + RMF_PREV_ADDR_OFFSET;
rrq_write_data <= std_logic_vector(resize(RRQ_MEMORY_MAX_ADDRESS,WORD_WIDTH));
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
-- READ Next Addr
when 11 =>
rrq_ready_out <= '1';
-- Memory Flow Control Guard
if (rrq_valid_out = '1') then
-- Set new Empty Head
rrq_empty_head_next <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH);
-- DONE
stage_next <= IDLE;
end if;
when others =>
null;
end case;
when SEND_RESPONSE =>
assert check_mask(mem_data_r.field_flags, GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG) severity FAILURE;
result_sel <= '1';
if (result_sel_ack = '1') then
start_r <= '1';
opcode_r <= SEND_RESPONSE;
if (ack_r = '1') then
stage_next <= WAIT_FOR_RET;
end if;
end if;
when WAIT_FOR_RET =>
assert check_mask(mem_data_r.field_flags, GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG) severity FAILURE;
result_sel <= '1';
if (done_r = '1') then
-- TODO: Propagate Error?
stage_next <= IDLE;
if (trigger_result = '1') then
stage_next <= CHECK_REQUESTS;
cnt_next <= 0;
end if;
end if;
when TRANSITION_STATE =>
assert (not (atomic_op_c = '1' and mem_data_r.addr = mem_data_c.addr)) severity FAILURE;
atomic_op_r <= '1';
case (cnt) is
-- CHECK Goal State
when 0 =>
-- Wait for Memory
if (mem_done = '1') then
assert check_mask(mem_data.field_flags, GMF_STATE_FLAG) severity FAILURE;
case (mem_data.state) is
when GoalStatus_package.STATUS_ACCEPTED =>
case (goal_state_latch) is
when GoalStatus_package.STATUS_EXECUTING =>
-- Synthesis Guard
if (TIMEOUT_DURATION /= ROS_TIME_INFINITE and is_terminal(goal_state_in)) then
cnt_next <= cnt + 1;
else
cnt_next <= cnt + 3;
end if;
when others =>
ack <= '1';
stage_next <= RETURN_USER;
return_code_latch_next <= ROS_RET_ACTION_GOAL_EVENT_INVALID;
end case;
when GoalStatus_package.STATUS_EXECUTING =>
case (goal_state_latch) is
when GoalStatus_package.STATUS_SUCCEEDED =>
-- Synthesis Guard
if (TIMEOUT_DURATION /= ROS_TIME_INFINITE and is_terminal(goal_state_in)) then
cnt_next <= cnt + 1;
else
cnt_next <= cnt + 3;
end if;
when GoalStatus_package.STATUS_ABORTED =>
-- Synthesis Guard
if (TIMEOUT_DURATION /= ROS_TIME_INFINITE and is_terminal(goal_state_in)) then
cnt_next <= cnt + 1;
else
cnt_next <= cnt + 3;
end if;
when others =>
ack <= '1';
stage_next <= RETURN_USER;
return_code_latch_next <= ROS_RET_ACTION_GOAL_EVENT_INVALID;
end case;
when GoalStatus_package.STATUS_CANCELING =>
case (goal_state_latch) is
when GoalStatus_package.STATUS_SUCCEEDED =>
-- Synthesis Guard
if (TIMEOUT_DURATION /= ROS_TIME_INFINITE and is_terminal(goal_state_in)) then
cnt_next <= cnt + 1;
else
cnt_next <= cnt + 3;
end if;
when GoalStatus_package.STATUS_ABORTED =>
-- Synthesis Guard
if (TIMEOUT_DURATION /= ROS_TIME_INFINITE and is_terminal(goal_state_in)) then
cnt_next <= cnt + 1;
else
cnt_next <= cnt + 3;
end if;
when GoalStatus_package.STATUS_CANCELED =>
-- Synthesis Guard
if (TIMEOUT_DURATION /= ROS_TIME_INFINITE and is_terminal(goal_state_in)) then
cnt_next <= cnt + 1;
else
cnt_next <= cnt + 3;
end if;
when others =>
ack <= '1';
stage_next <= RETURN_USER;
return_code_latch_next <= ROS_RET_ACTION_GOAL_EVENT_INVALID;
end case;
when others =>
ack <= '1';
stage_next <= RETURN_USER;
return_code_latch_next <= ROS_RET_ACTION_GOAL_EVENT_INVALID;
end case;
end if;
-- Time Addition 1/2
when 1 =>
-- Synthesis Guard
if (TIMEOUT_DURATION /= ROS_TIME_INFINITE) then
time_latch_next.sec <= std_logic_vector(unsigned(time.sec) + unsigned(TIMEOUT_DURATION.sec));
time_latch_next.nanosec <= std_logic_vector(unsigned(time.nanosec) + unsigned(TIMEOUT_DURATION.nanosec));
cnt_next <= cnt + 1;
end if;
-- Time Addition 2/2
when 2 =>
-- Synthesis Guard
if (TIMEOUT_DURATION /= ROS_TIME_INFINITE) then
if (unsigned(time_latch.nanosec) >= to_unsigned(10**9,time_latch.nanosec'length)) then
time_latch_next.sec <= std_logic_vector(unsigned(time_latch.sec) + 1);
time_latch_next.nanosec <= std_logic_vector(unsigned(time_latch.nanosec) - to_unsigned(10**9,time_latch.nanosec'length));
cnt_next <= cnt + 1;
end if;
end if;
-- UPDATE Goal State
when 3 =>
mem_start_r <= '1';
mem_opcode_r <= UPDATE;
mem_r.field_flags <= GMF_STATE_FLAG;
mem_r.addr <= unsigned(goal_handle_latch);
mem_r.state <= goal_state_latch;
-- Synthesis Guard
if (TIMEOUT_DURATION /= ROS_TIME_INFINITE and is_terminal(goal_state_in)) then
mem_r.field_flags <= GMF_STATE_FLAG or GMF_DEADLINE_FLAG;
mem_r.deadline <= time_latch;
-- Update Checktime
if (time_latch < check_time) then
check_time_next <= time_latch;
end if;
end if;
if (mem_ack_r = '1') then
if (is_terminal(goal_state_in)) then
trigger_result_next <= '1';
-- Update Terminal Goal Counter
terminal_cnt_next <= terminal_cnt + 1;
end if;
ack <= '1';
stage_next <= RETURN_USER;
return_code_latch_next <= ROS_RET_OK;
-- Trigger Status Update
trigger_status_r <= '1';
end if;
when others =>
null;
end case;
when GET_DATA =>
case (cnt) is
-- GET Data
when 0 =>
mem_start_r <= '1';
mem_opcode_r <= GET;
mem_r.field_flags <= GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG;
mem_r.addr <= mem_data_r.addr;
if (mem_ack_r = '1') then
cnt_next <= cnt + 1;
end if;
-- READ Data
when 1 =>
-- Wait for Memory
if (mem_done = '1') then
assert (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) severity FAILURE;
assert check_mask(mem_data.field_flags, GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG) severity FAILURE;
assert is_terminal(mem_data.state) severity FAILURE;
mem_data_r_next <= mem_data;
if (mem_data.state /= GoalStatus_package.STATUS_SUCCEEDED) then
-- NOTE: We are setting result index to MAX_GOALS to effectively "zero" the result.
-- This prevents sending the result of previous succeeded goals, if the current
-- goal is not succeeded.
mem_data_r_next.res_ind <= std_logic_vector(to_unsigned(MAX_GOALS, WORD_WIDTH));
end if;
stage_next <= CHECK_REQUESTS;
cnt_next <= 0;
end if;
when others =>
null;
end case;
when CHECK_REQUESTS =>
case (cnt) is
-- GET Goal Handle
when 0 =>
-- NOTE: We check the base here, because a REMOVE_REQUEST operation may reset the rrq_addr_base
if (rrq_addr_base = RRQ_MEMORY_MAX_ADDRESS) then
-- Reset
trigger_result_next <= '0';
-- DONE
stage_next <= IDLE;
else
rrq_valid_in <= '1';
rrq_addr <= rrq_addr_base + RMF_GOAL_HANDLE_OFFSET;
rrq_read <= '1';
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
end if;
-- GET Next Addr
when 1 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET;
rrq_read <= '1';
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
-- READ Goal Handle
when 2 =>
rrq_ready_out <= '1';
-- Memory Flow Control Guard
if (rrq_valid_out = '1') then
-- Found Result Request of Updated Goal
if (resize(unsigned(rrq_read_data),GOAL_MEMORY_ADDR_WIDTH) = mem_data_r.addr) then
stage_next <= GET_REQUEST_ID;
cnt_next <= 0;
rrq_abort_read <= '1';
else
cnt_next <= cnt + 1;
end if;
end if;
-- READ Next Addr
when 3 =>
rrq_ready_out <= '1';
-- Memory Flow Control Guard
if (rrq_valid_out = '1') then
rrq_addr_base_next <= resize(unsigned(rrq_read_data),GOAL_MEMORY_ADDR_WIDTH);
-- No More Stored Requests
if (resize(unsigned(rrq_read_data),GOAL_MEMORY_ADDR_WIDTH) = RRQ_MEMORY_MAX_ADDRESS) then
-- Reset
trigger_result_next <= '0';
stage_next <= IDLE;
else
-- Next Request
cnt_next <= 0;
end if;
end if;
when others =>
null;
end case;
when GET_REQUEST_ID =>
case (cnt) is
-- GET Request ID 1/6
when 0 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET;
rrq_read <= '1';
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
-- GET Request ID 2/6
when 1 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 1;
rrq_read <= '1';
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
-- GET Request ID 3/6
when 2 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 2;
rrq_read <= '1';
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
-- GET Request ID 4/6
when 3 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 3;
rrq_read <= '1';
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
-- GET Request ID 5/6
when 4 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 4;
rrq_read <= '1';
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
-- GET Request ID 6/6
when 5 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 5;
rrq_read <= '1';
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
-- READ Request ID 1/6
when 6 =>
rrq_ready_out <= '1';
-- Memory Flow Control Guard
if (rrq_valid_out = '1') then
request_id_latch_next.writer_guid(0) <= rrq_read_data;
cnt_next <= cnt + 1;
end if;
-- READ Request ID 2/6
when 7 =>
rrq_ready_out <= '1';
-- Memory Flow Control Guard
if (rrq_valid_out = '1') then
request_id_latch_next.writer_guid(1) <= rrq_read_data;
cnt_next <= cnt + 1;
end if;
-- READ Request ID 3/6
when 8 =>
rrq_ready_out <= '1';
-- Memory Flow Control Guard
if (rrq_valid_out = '1') then
request_id_latch_next.writer_guid(2) <= rrq_read_data;
cnt_next <= cnt + 1;
end if;
-- READ Request ID 4/6
when 9 =>
rrq_ready_out <= '1';
-- Memory Flow Control Guard
if (rrq_valid_out = '1') then
request_id_latch_next.writer_guid(3) <= rrq_read_data;
cnt_next <= cnt + 1;
end if;
-- READ Request ID 5/6
when 10 =>
rrq_ready_out <= '1';
-- Memory Flow Control Guard
if (rrq_valid_out = '1') then
request_id_latch_next.sequence_number(0) <= unsigned(rrq_read_data);
cnt_next <= cnt + 1;
end if;
-- READ Request ID 6/6
when 11 =>
rrq_ready_out <= '1';
-- Memory Flow Control Guard
if (rrq_valid_out = '1') then
request_id_latch_next.sequence_number(1) <= unsigned(rrq_read_data);
stage_next <= REMOVE_REQUEST;
cnt_next <= 0;
end if;
when others =>
null;
end case;
when REMOVE_REQUEST =>
-- NOTE: After this stage the rrq_addr_base is set to the Next request
case (cnt) is
-- GET Next Addr
when 0 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET;
rrq_read <= '1';
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
-- GET Prev Addr
when 1 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_addr_base + RMF_PREV_ADDR_OFFSET;
rrq_read <= '1';
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
-- SET Next Addr
when 2 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET;
rrq_write_data <= std_logic_vector(resize(rrq_empty_head,WORD_WIDTH));
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
-- Set New Empty Head
rrq_empty_head_next <= rrq_addr_base;
cnt_next <= cnt + 1;
end if;
-- READ Next Addr
when 3 =>
rrq_ready_out <= '1';
-- Memory Flow Control Guard
if (rrq_valid_out = '1') then
rrq_addr_latch_next <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH);
cnt_next <= cnt + 1;
end if;
-- READ Previous Addr
when 4 =>
rrq_ready_out <= '1';
-- Memory Flow Control Guard
if (rrq_valid_out = '1') then
if (rrq_addr_latch = RRQ_MEMORY_MAX_ADDRESS) then
if (resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH) = RRQ_MEMORY_MAX_ADDRESS) then
-- Reset Occupied Head
rrq_occupied_head_next <= RRQ_MEMORY_MAX_ADDRESS;
rrq_addr_base_next <= RRQ_MEMORY_MAX_ADDRESS;
stage_next <= SEND_RESPONSE;
else
rrq_addr_base_next <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH);
cnt_next <= cnt + 2;
end if;
else
rrq_addr_base_next <= rrq_addr_latch;
rrq_addr_latch_next <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH);
cnt_next <= cnt + 1;
end if;
end if;
-- SET Previous Addr (Next Slot)
when 6 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_addr_base + RMF_PREV_ADDR_OFFSET;
rrq_write_data <= std_logic_vector(rrq_addr_latch);
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
if (rrq_addr_latch = RRQ_MEMORY_MAX_ADDRESS) then
-- Set New Occupied Head
rrq_occupied_head_next <= rrq_addr_base;
stage_next <= SEND_RESPONSE;
else
rrq_addr_base_next <= rrq_addr_latch;
rrq_addr_latch_next <= rrq_addr_base;
cnt_next <= cnt + 1;
end if;
end if;
-- SET Next Addr (Previous Slot)
when 7 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET;
rrq_write_data <= std_logic_vector(rrq_addr_latch);
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
rrq_addr_base_next <= rrq_addr_latch;
stage_next <= SEND_RESPONSE;
end if;
when others =>
null;
end case;
when WAIT_FOR_MEM =>
-- Wait for Memory
if (mem_done = '1') then
assert check_mask(mem_data.field_flags, GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG) severity FAILURE;
mem_data_r_next <= mem_data;
stage_next <= RETURN_USER;
end if;
when RETURN_USER =>
-- End of Memory
if (mem_data_r.addr = GOAL_MEMORY_MAX_ADDRESS) then
goal_handle_out <= GOAL_HANDLE_UNKNOWN;
else
goal_handle_out <= std_logic_vector(resize(mem_data_r.addr, GOAL_HANDLE_WIDTH));
end if;
done <= '1';
return_code <= return_code_latch;
if (trigger_result = '1') then
stage_next <= GET_DATA;
cnt_next <= 0;
else
stage_next <= IDLE;
end if;
when CHECK_TIMEOUT =>
-- Synthesis Guard
if (TIMEOUT_DURATION /= ROS_TIME_INFINITE) then
case (cnt) is
-- GET
when 0 =>
mem_start_r <= '1';
mem_opcode_r <= GET;
mem_r.field_flags <= GMF_DEADLINE_FLAG;
mem_r.addr <= mem_data_r.addr;
if (mem_ack_r = '1') then
cnt_next <= cnt + 2;
end if;
-- GET NEXT
when 1 =>
mem_start_r <= '1';
mem_opcode_r <= GET_NEXT;
mem_r.field_flags <= GMF_DEADLINE_FLAG;
mem_r.addr <= mem_data_r.addr;
if (mem_ack_r = '1') then
cnt_next <= cnt + 1;
end if;
-- CHECK
when 2 =>
-- Wait for Memory
if (mem_done = '1') then
assert check_mask(mem_data.field_flags, GMF_DEADLINE_FLAG) severity FAILURE;
mem_data_r_next <= mem_data;
-- No more Goals
if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then
-- DONE
stage_next <= IDLE;
else
-- Timeout
if (mem_data.deadline <= time) then
cnt_next <= cnt + 1;
else
-- Update check Time
if (mem_data.deadline < check_time) then
check_time_next <= mem_data.deadline;
end if;
cnt_next <= 1;
end if;
end if;
end if;
-- REMOVE
when 3 =>
-- TODO: This needs to take into account the other FSMs (Status, Cancel, Goal)
mem_start_r <= '1';
mem_opcode_r <= REMOVE;
mem_r.addr <= mem_data_r.addr;
if (mem_ack_r = '1') then
cnt_next <= cnt + 1;
-- Trigger Status Update
trigger_status_r <= '1';
-- Update Terminal Goal Counter
terminal_cnt_next <= terminal_cnt - 1;
end if;
-- Wait for Remove
when 4 =>
-- Wait for Memory
if (mem_done = '1') then
mem_data_r_next <= mem_data;
-- No more Goals
if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then
if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then
-- DONE
stage_next <= IDLE;
else
mem_data_r.addr <= mem_occupied_head;
cnt_next <= 1;
end if;
else
cnt_next <= 1;
end if;
end if;
when others =>
null;
end case;
end if;
when REMOVE_OLDEST =>
-- Synthesis Guard
if (TIMEOUT_DURATION = ROS_TIME_INFINITE) then
case (cnt) is
-- GET
when 0 =>
mem_start_r <= '1';
mem_opcode_r <= GET;
mem_r.field_flags <= GMF_STATE_FLAG;
mem_r.addr <= mem_data_r.addr;
if (mem_ack_r = '1') then
cnt_next <= cnt + 2;
end if;
-- GET NEXT
when 1 =>
mem_start_r <= '1';
mem_opcode_r <= GET_PREV;
mem_r.field_flags <= GMF_STATE_FLAG;
mem_r.addr <= mem_data_r.addr;
if (mem_ack_r = '1') then
cnt_next <= cnt + 1;
end if;
-- CHECK
when 2 =>
-- Wait for Memory
if (mem_done = '1') then
assert check_mask(mem_data.field_flags, GMF_STATE_FLAG) severity FAILURE;
mem_data_r_next <= mem_data;
-- No more Goals
if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then
-- DONE
stage_next <= IDLE;
else
if (is_terminal(mem_data.state)) then
cnt_next <= cnt + 1;
else
cnt_next <= 1;
end if;
end if;
end if;
-- REMOVE
when 3 =>
mem_start_r <= '1';
mem_opcode_r <= REMOVE;
mem_r.addr <= mem_data_r.addr;
if (mem_ack_r = '1') then
cnt_next <= cnt + 1;
-- Trigger Status Update
trigger_status_r <= '1';
-- Update Terminal Goal Counter
terminal_cnt_next <= terminal_cnt - 1;
end if;
-- Wait for Remove
when 4 =>
-- Wait for Memory
if (mem_done = '1') then
mem_data_r_next <= mem_data;
-- No more Goals
if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then
-- DONE
stage_next <= IDLE;
else
-- NOTE: After Removal, mem_data.addr is set to the previous goal, so we need to call GET and not GET_PREV
cnt_next <= 0;
end if;
end if;
when others =>
null;
end case;
end if;
when PUBLISH_FEEDBACK =>
case (cnt) is
-- Wait for Data
when 0 =>
if (mem_done = '1') then
assert check_mask(mem_data.field_flags, GMF_GOAL_ID_FLAG) severity FAILURE;
mem_data_r_next <= mem_data_r;
cnt_next <= cnt + 1;
end if;
-- Start Publish
when 1 =>
start_fb <= '1';
opcode_fb <= PUBLISH;
if (ack_fb = '1') then
cnt_next <= cnt + 1;
end if;
-- Wait for Publish
when 2 =>
-- Passthrough
done <= done_fb;
return_code <= return_code_fb;
if (done_fb = '1') then
-- DONE
stage_next <= IDLE;
end if;
when others =>
null;
end case;
when RESET_MEMORY =>
case (cnt) is
-- SET Next Pointer
when 0 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET;
if (rrq_addr_base = MAX_RRQ_ADDRESS) then
rrq_write_data <= std_logic_vector(resize(RRQ_MEMORY_MAX_ADDRESS,WORD_WIDTH));
else
rrq_write_data <= std_logic_vector(resize(rrq_addr_base + RRQ_FRAME_SIZE,WORD_WIDTH));
end if;
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
cnt_next <= cnt + 1;
end if;
-- SET Previous Pointer
when 1 =>
rrq_valid_in <= '1';
rrq_addr <= rrq_addr_base + RMF_PREV_ADDR_OFFSET;
rrq_write_data <= std_logic_vector(resize(rrq_addr_latch,WORD_WIDTH));
-- Memory Flow Control Guard
if (rrq_ready_in = '1') then
if (rrq_addr_base = MAX_RRQ_ADDRESS) then
-- Initialize Empty and Occupied Heads
rrq_empty_head <= FIRST_RRQ_ADDRESS;
rrq_occupied_head <= RRQ_MEMORY_MAX_ADDRESS;
-- DONE
stage_next <= IDLE;
else
rrq_addr_latch_next <= rrq_addr_base;
rrq_addr_base_next <= rrq_addr_base + RRQ_FRAME_SIZE;
cnt_next <= 0;
end if;
end if;
when others =>
null;
end case;
end case;
end process;
-- *Goal State Machine*
-- STATE DESCRIPTION
-- IDLE Idle state
-- WAIT_FOR_DATA Wait goal service request
-- WAIT_FOR_USER Wait for user input
-- ADD_GOAL Add goal to memory
-- SEND_RESPONSE Send goal service response
-- WAIT_FOR_RET Wait for goal service response
goal_prc : process(all)
begin
-- DEFAULT
goal_stage_next <= goal_stage;
g_stamp_next <= g_stamp;
g_accept_next <= g_accept;
-- DEFAULT Unregistered
trigger_status_g <= '0';
mem_start_g <= '0';
mem_opcode_g <= NOP;
start_g <= '0';
opcode_g <= NOP;
new_goal_request <= '0';
new_goal_handle <= GOAL_HANDLE_UNKNOWN;
new_goal_result_index <= (others => '0');
case (goal_stage) is
when IDLE =>
if (data_available_g = '1' and mem_empty_head /= GOAL_MEMORY_MAX_ADDRESS) then
start_g <= '1';
opcode_g <= TAKE_REQUEST;
if (ack_g = '1') then
goal_stage_next <= WAIT_FOR_DATA;
end if;
end if;
when WAIT_FOR_DATA =>
if (done_g = '1') then
case (return_code_g) is
when ROS_RET_OK =>
if (taken_g = '1') then
goal_stage_next <= WAIT_FOR_USER;
else
goal_stage_next <= IDLE;
end if;
when others =>
-- TODO: Propagate Error?
goal_stage_next <= IDLE;
end case;
end if;
when WAIT_FOR_USER =>
new_goal_request <= '1';
new_goal_handle <= std_logic_vector(resize(mem_empty_head,GOAL_HANDLE_WIDTH));
new_goal_result_index <= std_logic_vector(empty_head_res_ind);
if (new_goal_response = '1') then
g_stamp_next <= time;
if (new_goal_accepted = '1') then
goal_stage_next <= ADD_GOAL;
g_accept_next <= '1';
else
goal_stage_next <= SEND_RESPONSE;
g_accept_next <= '0';
end if;
end if;
when ADD_GOAL =>
assert (mem_empty_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE;
mem_start_g <= '1';
mem_opcode_g <= INSERT;
if (mem_ack_g = '1') then
goal_stage_next <= SEND_RESPONSE;
-- Trigger Status Update
trigger_status_g <= '1';
end if;
when SEND_RESPONSE =>
start_g <= '1';
opcode_g <= SEND_RESPONSE;
if (ack_g = '1') then
goal_stage_next <= WAIT_FOR_RET;
end if;
when WAIT_FOR_RET =>
if (done_g = '1') then
-- TODO: Propagate Error?
goal_stage_next <= IDLE;
end if;
end case;
end process;
-- *Cancel State Machine*
-- STATE DESCRIPTION
-- IDLE Idle state
-- WAIT_FOR_DATA Wait for cancel service request
-- GET Get goal data from memory (Pointed by mem_data_c.addr)
-- GET_NEXT Get goal data from memory (Next goal from one pointed by mem_data_c.addr)
-- CHECK Determine what data has to be checked for each stored goal
-- CHECK_GOAL_ID Check if cancel goal id is same as mem_data_c
-- CHECK_STAMP Check if cancel goal stamp is earlier than the one in mem_data_c
-- CHECK_STATE Check if mem_data_c is in terminal state
-- WAIT_FOR_USER Wait for user input
-- CANCEL_GOAL Update goal state to CANCELING
-- ADD_CANCEL Store canceling goal for later response
-- SEND_RESPONSE Send cancel service response
-- WAIT_FOR_RET Wait for cancel service response
cancel_prc : process(all)
begin
-- DEFAULT
cancel_stage_next <= cancel_stage;
c_cnt_next <= c_cnt;
mem_data_c_next <= mem_data_c;
search_type_c_next <= search_type_c;
goals_canceling_cnt_next <= goals_canceling_cnt;
cancel_ret_code_next <= cancel_ret_code;
-- DEFAULT Unregistered
atomic_op_c <= '0';
trigger_status_c <= '0';
mem_start_c <= '0';
mem_opcode_c <= NOP;
start_c <= '0';
opcode_c <= NOP;
mem_c <= ZERO_GOAL_DATA;
cancel_request <= '0';
cancel_request_handle <= GOAL_HANDLE_UNKNOWN;
goals_canceling_addr_c <= (others => '0');
goals_canceling_ren_c <= '0';
goals_canceling_wen_c <= '0';
goals_canceling_ack_c <= '0';
goals_canceling_goal_id_w_c <= (others => '0');
goals_canceling_stamp_w_c <= (others => '0');
case (cancel_stage) is
when IDLE =>
if (data_available_c = '1') then
start_c <= '1';
opcode_c <= TAKE_REQUEST;
if (ack_c = '1') then
cancel_stage_next <= WAIT_FOR_DATA;
--Reset
goals_canceling_cnt_next <= 0;
end if;
end if;
when WAIT_FOR_DATA =>
if (done_c = '1') then
case (return_code_c) is
when ROS_RET_OK =>
if (taken_c = '1') then
-- No Goals Available
if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then
cancel_ret_code_next <= CancelGoal_package.RR_ERROR_REJECTED;
cancel_stage_next <= SEND_RESPONSE;
else
if (to_UUID(goal_info_goal_id_c) = UUID_UNKNOWN) then
if (to_ROS_TIME(goal_info_stamp_c) = ROS_TIME_ZERO) then
search_type_c_next <= SEARCH_NONE;
else
search_type_c_next <= SEARCH_STAMP;
end if;
else
search_type_c_next <= SEARCH_GOAL_ID;
end if;
cancel_stage_next <= GET;
mem_data_c_next.addr <= mem_occupied_head;
end if;
else
cancel_stage_next <= IDLE;
end if;
when others =>
-- TODO: Propagate Error?
cancel_stage_next <= IDLE;
end case;
end if;
when GET =>
-- Atomic Operation Guard
if (not (atomic_op_r = '1' and mem_data_c.addr = mem_data_r.addr)) then
mem_start_c <= '1';
mem_opcode_c <= GET;
mem_c.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG;
mem_c.addr <= mem_data_c.addr;
if (mem_ack_c = '1') then
cancel_stage_next <= CHECK;
end if;
end if;
when GET_NEXT =>
mem_start_c <= '1';
mem_opcode_c <= GET_NEXT;
mem_c.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG;
mem_c.addr <= mem_data_c.addr;
if (mem_ack_c = '1') then
cancel_stage_next <= CHECK;
end if;
when CHECK =>
atomic_op_c <= '1';
-- Wait for Memory
if (mem_done = '1') then
-- End of Memory
if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then
case (search_type_c) is
when SEARCH_GOAL_ID =>
cancel_ret_code_next <= CancelGoal_package.RR_ERROR_UNKNOWN_GOAL_ID;
cancel_stage_next <= SEND_RESPONSE;
when others =>
-- No Goals Canceled
if (goals_canceling_cnt = 0) then
cancel_ret_code_next <= CancelGoal_package.RR_ERROR_REJECTED;
cancel_stage_next <= SEND_RESPONSE;
else
cancel_ret_code_next <= CancelGoal_package.RR_ERROR_NONE;
cancel_stage_next <= SEND_RESPONSE;
end if;
end case;
else
assert check_mask(mem_data.field_flags, GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG) severity FAILURE;
mem_data_c_next <= mem_data;
-- NOTE: If both the Main and Cancel FSM enter the atomic operation zone at the same time, the cancel FSM gives way and waits.
if (atomic_op_r = '1' and mem_data.addr = mem_data_r.addr) then
cancel_stage_next <= GET;
else
case (search_type_c) is
when SEARCH_GOAL_ID =>
cancel_stage_next <= CHECK_GOAL_ID;
c_cnt_next <= 0;
when SEARCH_STAMP =>
cancel_stage_next <= CHECK_STAMP;
when SEARCH_NONE =>
cancel_stage_next <= CHECK_STATE;
end case;
end if;
end if;
end if;
when CHECK_GOAL_ID =>
assert (not (atomic_op_r = '1' and mem_data_c.addr = mem_data_r.addr)) severity FAILURE;
atomic_op_c <= '1';
case (c_cnt) is
-- GOAL_ID 1/4
when 0 =>
if (mem_data_c.goal_id(0) = to_guid(goal_info_goal_id_c)(0)) then
c_cnt_next <= c_cnt + 1;
else
cancel_stage_next <= GET_NEXT;
end if;
-- GOAL_ID 2/4
when 1 =>
if (mem_data_c.goal_id(1) = to_guid(goal_info_goal_id_c)(1)) then
c_cnt_next <= c_cnt + 1;
else
cancel_stage_next <= GET_NEXT;
end if;
-- GOAL_ID 3/4
when 2 =>
if (mem_data_c.goal_id(2) = to_guid(goal_info_goal_id_c)(2)) then
c_cnt_next <= c_cnt + 1;
else
cancel_stage_next <= GET_NEXT;
end if;
-- GOAL_ID 4/4
when 3 =>
if (mem_data_c.goal_id(3) = to_guid(goal_info_goal_id_c)(3)) then
cancel_stage_next <= CHECK_STATE;
c_cnt_next <= 0;
else
cancel_stage_next <= GET_NEXT;
end if;
when others =>
null;
end case;
when CHECK_STAMP =>
assert (not (atomic_op_r = '1' and mem_data_c.addr = mem_data_r.addr)) severity FAILURE;
atomic_op_c <= '1';
if (mem_data_c.stamp < to_ROS_TIME(goal_info_stamp_c)) then
cancel_stage_next <= CHECK_STATE;
else
cancel_stage_next <= GET_NEXT;
end if;
when CHECK_STATE =>
assert (not (atomic_op_r = '1' and mem_data_c.addr = mem_data_r.addr)) severity FAILURE;
atomic_op_c <= '1';
if (is_terminal(mem_data_c.state)) then
cancel_ret_code_next <= CancelGoal_package.RR_ERROR_GOAL_TERMINATED;
cancel_stage_next <= SEND_RESPONSE;
elsif (mem_data_c.state = GoalStatus_package.STATUS_CANCELING) then
-- Skip State Change
cancel_stage_next <= ADD_CANCEL;
else
cancel_stage_next <= WAIT_FOR_USER;
end if;
when WAIT_FOR_USER =>
assert (not (atomic_op_r = '1' and mem_data_c.addr = mem_data_r.addr)) severity FAILURE;
atomic_op_c <= '1';
cancel_request <= '1';
cancel_request_handle <= std_logic_vector(mem_data_c.addr);
if (cancel_response = '1') then
if (cancel_accepted = '1') then
cancel_stage_next <= CANCEL_GOAL;
else
case (search_type_c) is
when SEARCH_GOAL_ID =>
if (to_ROS_TIME(goal_info_stamp_c) = ROS_TIME_ZERO) then
-- No Goals Canceled
cancel_ret_code_next <= CancelGoal_package.RR_ERROR_REJECTED;
cancel_stage_next <= SEND_RESPONSE;
else
-- Search for Timestamps
search_type_c_next <= SEARCH_STAMP;
cancel_stage_next <= GET;
mem_data_c_next.addr <= mem_occupied_head;
end if;
when SEARCH_STAMP =>
-- NOTE: Since the goals are added chronologically, we don't have to check the timestamps from now on
search_type_c_next <= SEARCH_NONE;
cancel_stage_next <= GET_NEXT;
when SEARCH_NONE =>
-- Continue
cancel_stage_next <= GET_NEXT;
end case;
end if;
end if;
when CANCEL_GOAL =>
assert (not (atomic_op_r = '1' and mem_data_c.addr = mem_data_r.addr)) severity FAILURE;
atomic_op_c <= '1';
mem_start_c <= '1';
mem_opcode_c <= UPDATE;
mem_c.addr <= mem_data_c.addr;
mem_c.field_flags <= GMF_STATE_FLAG;
mem_c.state <= GoalStatus_package.STATUS_CANCELING;
if (mem_ack_c = '1') then
cancel_stage_next <= ADD_CANCEL;
-- Trigger Status Update
trigger_status_c <= '1';
end if;
when ADD_CANCEL =>
goals_canceling_addr_c <= std_logic_vector(to_unsigned(goals_canceling_cnt,CancelGoal_package.RR_GOALS_CANCELING_GOAL_ID_ADDR_WIDTH));
goals_canceling_wen_c <= '1';
goals_canceling_goal_id_w_c <= std_logic_vector(to_unsigned(mem_data_c.goal_id));
goals_canceling_stamp_w_c <= std_logic_vector(to_unsigned(mem_data_c.stamp));
if (goals_canceling_ready_c = '1') then
-- Reached maximum number of cancel goals
if (goals_canceling_cnt = CancelGoal_package.RR_GOALS_CANCELING_MAX_DEPTH) then
-- DONE
cancel_ret_code_next <= CancelGoal_package.RR_ERROR_NONE;
cancel_stage_next <= SEND_RESPONSE;
else
goals_canceling_cnt_next <= goals_canceling_cnt + 1;
case (search_type_c) is
when SEARCH_GOAL_ID =>
if (to_ROS_TIME(goal_info_stamp_c) = ROS_TIME_ZERO) then
-- DONE
cancel_ret_code_next <= CancelGoal_package.RR_ERROR_NONE;
cancel_stage_next <= SEND_RESPONSE;
else
assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE;
-- Search for Timestamps
search_type_c_next <= SEARCH_STAMP;
cancel_stage_next <= GET;
mem_data_c_next.addr <= mem_occupied_head;
end if;
when SEARCH_STAMP =>
-- NOTE: Since the goals are added chronologically, we don't have to check the timestamps from now on
search_type_c_next <= SEARCH_NONE;
cancel_stage_next <= GET_NEXT;
when SEARCH_NONE =>
-- Continue
cancel_stage_next <= GET_NEXT;
end case;
end if;
end if;
when SEND_RESPONSE =>
start_c <= '1';
opcode_c <= SEND_RESPONSE;
if (ack_c = '1') then
cancel_stage_next <= WAIT_FOR_RET;
end if;
when WAIT_FOR_RET =>
if (done_c = '1') then
-- TODO: Propagate Error?
cancel_stage_next <= IDLE;
end if;
when others =>
null;
end case;
end process;
-- *Status State Machine*
-- STATE DESCRIPTION
-- IDLE Idle state
-- GET_FIRST Get first goal data from memory
-- GET_NEXT Get goal data from memory (Next goal from one pointed by mem_data_c.addr)
-- WAIT_FOR_DATA Wait for memory data
-- PUSH_STATUS Add goal data to status list
-- PUBLISH Publish status list
-- WAIT_FOR_PUBLISHER Wait for status list publisher
status_prc : process (all)
begin
-- DEFAULT
status_stage_next <= status_stage;
trigger_status_next <= trigger_status;
status_list_cnt_next <= status_list_cnt;
mem_data_s_next <= mem_data_s;
-- DEFAULT Unregistered
status_idle_sig <= '0';
start_s <= '0';
opcode_s <= NOP;
mem_start_s <= '0';
mem_opcode_s <= NOP;
mem_s <= ZERO_GOAL_DATA;
status_list_ren_s <= '0';
status_list_wen_s <= '0';
status_list_ack_s <= '0';
status_list_addr_s <= (others => '0');
status_list_goal_info_goal_id_w_s <= std_logic_vector(to_unsigned(UUID_UNKNOWN));
status_list_goal_info_stamp_w_s <= std_logic_vector(to_unsigned(ROS_TIME_INVALID));
status_list_status_w_s <= GoalStatus_package.STATUS_UNKNOWN;
-- Trigger Status Setter
if (trigger_status_c = '1' or trigger_status_g = '1' or trigger_status_r = '1') then
trigger_status_next <= '1';
end if;
case (status_stage) is
when IDLE =>
status_idle_sig <= '1';
if (trigger_status = '1') then
-- Trigger Status Resetter
trigger_status_next <= '0';
-- Reset
status_list_cnt_next <= 0;
if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then
status_stage_next <= PUBLISH;
else
status_stage_next <= GET_FIRST;
end if;
end if;
when GET_FIRST =>
assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE;
mem_start_s <= '1';
mem_opcode_s <= GET;
mem_s.addr <= mem_occupied_head;
mem_s.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG;
if (mem_ack_s = '1') then
status_stage_next <= WAIT_FOR_DATA;
end if;
when GET_NEXT =>
mem_start_s <= '1';
mem_opcode_s <= GET_NEXT;
mem_s.addr <= mem_data_s.addr;
mem_s.field_flags <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG;
if (mem_ack_s = '1') then
status_stage_next <= WAIT_FOR_DATA;
end if;
when WAIT_FOR_DATA =>
-- Wait for DATA
if (mem_done = '1') then
mem_data_s_next <= mem_data_s;
-- No more Goals
if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then
status_stage_next <= PUBLISH;
else
status_stage_next <= PUSH_STATUS;
end if;
end if;
when PUSH_STATUS =>
status_list_addr_s <= std_logic_vector(to_unsigned(status_list_cnt,GoalStatusArray_package.STATUS_LIST_GOAL_INFO_GOAL_ID_ADDR_WIDTH));
status_list_wen_s <= '1';
status_list_goal_info_goal_id_w_s <= std_logic_vector(to_unsigned(mem_data_s.goal_id));
status_list_goal_info_stamp_w_s <= std_logic_vector(to_unsigned(mem_data_s.stamp));
status_list_status_w_s <= mem_data_s.state;
if (status_list_ready_s = '1') then
status_list_cnt_next <= status_list_cnt + 1;
if (status_list_cnt = GoalStatusArray_package.STATUS_LIST_MAX_DEPTH-1) then
status_stage_next <= PUBLISH;
else
status_stage_next <= GET_NEXT;
end if;
end if;
when PUBLISH =>
start_s <= '1';
opcode_s <= PUBLISH;
if (ack_s = '1') then
status_stage_next <= WAIT_FOR_PUBLISHER;
end if;
when WAIT_FOR_PUBLISHER =>
if (done_s = '1') then
-- TODO: Propagate Error?
status_stage_next <= IDLE;
end if;
when others =>
null;
end case;
end process;
-- *Goal Memory State Machine*
-- STATE DESCRIPTION
-- IDLE Idle State. Done Signal is pulled high and Memory FSM accepts new memory operations
-- INSERT See Memory OPCODE Description
-- GET See Memory OPCODE Description
-- GET_NEXT See Memory OPCODE Description
-- GET_PREV See Memory OPCODE Description
-- UPDATE See Memory OPCODE Description
-- REMOVE See Memory OPCODE Description
-- RESET_MEMORY Reset goal memory to empty state
mem_ctrl_prc : process(all)
begin
-- DEFAULT
mem_stage_next <= mem_stage;
mem_cnt_next <= mem_cnt;
mem_occupied_head_next <= mem_occupied_head;
mem_occupied_tail_next <= mem_occupied_tail;
mem_empty_head_next <= mem_empty_head;
mem_empty_tail_next <= mem_empty_tail;
empty_head_res_ind_next <= empty_head_res_ind;
mem_latch_data_next <= mem_latch_data;
mem_data_next <= mem_data;
mem_addr_base_next <= mem_addr_base;
mem_addr_latch_next <= mem_addr_latch;
-- DEFAULT unregistered
mem_ack_g <= '0';
mem_ack_r <= '0';
mem_ack_c <= '0';
mem_ack_s <= '0';
mem_done <= '0';
mem_abort_read <= '0';
mem_ready_out <= '0';
mem_valid_in <= '0';
mem_read <= '0';
mem_done <= '0';
mem_addr <= (others => '0');
mem_write_data <= (others => '0');
case (mem_stage) is
-- NOTE: The REMOVE opcode sets mem_data.addr to the previous slot (or GOAL_MEMORY_MAX_ADDRESS if no previous goals)
-- This allows defined behaviours of GET_NEXT when removing goals.
when IDLE =>
mem_done <= '1';
if (mem_start_g = '1') then
mem_ack_g <= '1';
case (mem_opcode_g) is
when INSERT =>
assert (mem_empty_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE;
-- Latch DATA
mem_latch_data_next.goal_id <= to_guid(goal_id_g);
mem_latch_data_next.stamp <= g_stamp;
mem_addr_base_next <= mem_empty_head;
mem_stage_next <= INSERT;
mem_cnt_next <= 0;
when others =>
assert FALSE severity FAILURE;
end case;
elsif (mem_start_r = '1') then
mem_ack_r <= '1';
case (mem_opcode_r) is
when GET =>
-- Latch DATA
mem_latch_data_next <= mem_r;
if (mem_r.addr = mem_addr_base) then
mem_data_next.field_flags <= mem_data.field_flags or mem_r.field_flags;
else
mem_data_next <= ZERO_GOAL_DATA;
mem_data_next.field_flags <= mem_r.field_flags;
end if;
mem_addr_base_next <= mem_r.addr;
mem_stage_next <= GET;
if check_mask(mem_r.field_flags,GMF_STATE_FLAG) then
mem_cnt_next <= 0;
elsif check_mask(mem_r.field_flags,GMF_GOAL_ID_FLAG) then
mem_cnt_next <= 1;
elsif check_mask(mem_r.field_flags,GMF_STAMP_FLAG) then
mem_cnt_next <= 5;
elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_r.field_flags,GMF_DEADLINE_FLAG)) then
mem_cnt_next <= 7;
elsif check_mask(mem_r.field_flags,GMF_RESULT_INDEX_FLAG) then
mem_cnt_next <= 9;
else
-- DONE
mem_stage_next <= IDLE;
end if;
when GET_NEXT =>
-- Latch DATA
mem_latch_data_next <= mem_r;
mem_data_next <= ZERO_GOAL_DATA;
mem_data_next.field_flags <= mem_r.field_flags;
mem_addr_base_next <= mem_r.addr;
mem_stage_next <= GET_NEXT;
mem_cnt_next <= 0;
when GET_PREV =>
-- Latch DATA
mem_latch_data_next <= mem_r;
mem_data_next <= ZERO_GOAL_DATA;
mem_data_next.field_flags <= mem_r.field_flags;
mem_addr_base_next <= mem_r.addr;
mem_stage_next <= GET_PREV;
mem_cnt_next <= 0;
when UPDATE =>
-- Latch DATA
mem_latch_data_next <= mem_r;
if (mem_r.addr = mem_addr_base) then
mem_data_next.field_flags <= mem_data.field_flags or mem_r.field_flags;
else
mem_data_next <= ZERO_GOAL_DATA;
mem_data_next.field_flags <= mem_r.field_flags;
end if;
mem_addr_base_next <= mem_r.addr;
mem_stage_next <= UPDATE;
if check_mask(mem_r.field_flags,GMF_STATE_FLAG) then
mem_cnt_next <= 0;
elsif check_mask(mem_r.field_flags,GMF_GOAL_ID_FLAG) then
mem_cnt_next <= 1;
elsif check_mask(mem_r.field_flags,GMF_STAMP_FLAG) then
mem_cnt_next <= 5;
elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_r.field_flags,GMF_DEADLINE_FLAG)) then
mem_cnt_next <= 7;
elsif check_mask(mem_r.field_flags,GMF_RESULT_INDEX_FLAG) then
mem_cnt_next <= 9;
else
-- DONE
mem_stage_next <= IDLE;
end if;
when REMOVE =>
mem_addr_base_next <= mem_r.addr;
mem_stage_next <= REMOVE;
mem_cnt_next <= 0;
when others =>
assert FALSE severity FAILURE;
end case;
elsif (mem_start_c = '1') then
mem_ack_c <= '1';
case (mem_opcode_c) is
when GET =>
-- Latch DATA
mem_latch_data_next <= mem_c;
if (mem_c.addr = mem_addr_base) then
mem_data_next.field_flags <= mem_data.field_flags or mem_c.field_flags;
else
mem_data_next <= ZERO_GOAL_DATA;
mem_data_next.field_flags <= mem_c.field_flags;
end if;
mem_addr_base_next <= mem_c.addr;
mem_stage_next <= GET;
if check_mask(mem_c.field_flags,GMF_STATE_FLAG) then
mem_cnt_next <= 0;
elsif check_mask(mem_c.field_flags,GMF_GOAL_ID_FLAG) then
mem_cnt_next <= 1;
elsif check_mask(mem_c.field_flags,GMF_STAMP_FLAG) then
mem_cnt_next <= 5;
elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_c.field_flags,GMF_DEADLINE_FLAG)) then
mem_cnt_next <= 7;
elsif check_mask(mem_c.field_flags,GMF_RESULT_INDEX_FLAG) then
mem_cnt_next <= 9;
else
-- DONE
mem_stage_next <= IDLE;
end if;
when GET_NEXT =>
-- Latch DATA
mem_latch_data_next <= mem_c;
mem_data_next <= ZERO_GOAL_DATA;
mem_data_next.field_flags <= mem_c.field_flags;
mem_addr_base_next <= mem_c.addr;
mem_stage_next <= GET_NEXT;
mem_cnt_next <= 0;
when GET_PREV =>
-- Latch DATA
mem_latch_data_next <= mem_c;
mem_data_next <= ZERO_GOAL_DATA;
mem_data_next.field_flags <= mem_c.field_flags;
mem_addr_base_next <= mem_c.addr;
mem_stage_next <= GET_PREV;
mem_cnt_next <= 0;
when UPDATE =>
-- Latch DATA
mem_latch_data_next <= mem_c;
if (mem_c.addr = mem_addr_base) then
mem_data_next.field_flags <= mem_data.field_flags or mem_c.field_flags;
else
mem_data_next <= ZERO_GOAL_DATA;
mem_data_next.field_flags <= mem_c.field_flags;
end if;
mem_addr_base_next <= mem_c.addr;
mem_stage_next <= UPDATE;
if check_mask(mem_c.field_flags,GMF_STATE_FLAG) then
mem_cnt_next <= 0;
elsif check_mask(mem_c.field_flags,GMF_GOAL_ID_FLAG) then
mem_cnt_next <= 1;
elsif check_mask(mem_c.field_flags,GMF_STAMP_FLAG) then
mem_cnt_next <= 5;
elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_c.field_flags,GMF_DEADLINE_FLAG)) then
mem_cnt_next <= 7;
elsif check_mask(mem_c.field_flags,GMF_RESULT_INDEX_FLAG) then
mem_cnt_next <= 9;
else
-- DONE
mem_stage_next <= IDLE;
end if;
when others =>
assert FALSE severity FAILURE;
end case;
elsif (mem_start_s = '1') then
mem_ack_s <= '1';
case (mem_opcode_s) is
when GET =>
-- Latch DATA
mem_latch_data_next <= mem_s;
if (mem_s.addr = mem_addr_base) then
mem_data_next.field_flags <= mem_data.field_flags or mem_s.field_flags;
else
mem_data_next <= ZERO_GOAL_DATA;
mem_data_next.field_flags <= mem_s.field_flags;
end if;
mem_addr_base_next <= mem_s.addr;
mem_stage_next <= GET;
if check_mask(mem_s.field_flags,GMF_STATE_FLAG) then
mem_cnt_next <= 0;
elsif check_mask(mem_s.field_flags,GMF_GOAL_ID_FLAG) then
mem_cnt_next <= 1;
elsif check_mask(mem_s.field_flags,GMF_STAMP_FLAG) then
mem_cnt_next <= 5;
elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_s.field_flags,GMF_DEADLINE_FLAG)) then
mem_cnt_next <= 7;
elsif check_mask(mem_s.field_flags,GMF_RESULT_INDEX_FLAG) then
mem_cnt_next <= 9;
else
-- DONE
mem_stage_next <= IDLE;
end if;
when GET_NEXT =>
-- Latch DATA
mem_latch_data_next <= mem_s;
mem_data_next <= ZERO_GOAL_DATA;
mem_data_next.field_flags <= mem_s.field_flags;
mem_addr_base_next <= mem_s.addr;
mem_stage_next <= GET_NEXT;
mem_cnt_next <= 0;
when GET_PREV =>
-- Latch DATA
mem_latch_data_next <= mem_s;
mem_data_next <= ZERO_GOAL_DATA;
mem_data_next.field_flags <= mem_s.field_flags;
mem_addr_base_next <= mem_s.addr;
mem_stage_next <= GET_PREV;
mem_cnt_next <= 0;
when others =>
assert FALSE severity FAILURE;
end case;
end if;
when INSERT =>
case (mem_cnt) is
-- GET Next Addr
when 0 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET;
mem_read <= '1';
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- READ Next Addr
when 1 =>
mem_ready_out <= '1';
-- Memory Flow Control Guard
if (mem_valid_out = '1') then
-- SET New Empty Head
mem_empty_head_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH);
mem_cnt_next <= mem_cnt + 1;
end if;
-- GET Result Index (New Empty Head)
when 2 =>
mem_valid_in <= '1';
mem_addr <= mem_empty_head + GMF_RESULT_INDEX_OFFSET;
mem_read <= '1';
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- READ Result Index (New Empty Head)
when 3 =>
mem_ready_out <= '1';
-- Memory Flow Control Guard
if (mem_valid_out = '1') then
empty_head_res_ind_next <= unsigned(mem_read_data);
mem_cnt_next <= mem_cnt + 1;
end if;
-- SET Goal State
when 4 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_STATE_OFFSET;
mem_write_data(CDR_INT8_WIDTH-1 downto 0) <= GoalStatus_package.STATUS_ACCEPTED;
mem_data_next.state <= GoalStatus_package.STATUS_ACCEPTED;
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- SET Goal ID 1/4
when 5 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET;
mem_write_data <= mem_latch_data.goal_id(0);
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- SET Goal ID 2/4
when 6 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 1;
mem_write_data <= mem_latch_data.goal_id(1);
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- SET Goal ID 3/4
when 7 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 2;
mem_write_data <= mem_latch_data.goal_id(2);
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- SET Goal ID 4/4
when 8 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 3;
mem_write_data <= mem_latch_data.goal_id(3);
mem_data_next.goal_id <= mem_latch_data.goal_id;
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- SET Stamp 1/2
when 9 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_STAMP_OFFSET;
mem_write_data <= mem_latch_data.stamp.sec;
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- SET Stamp 2/2
when 10 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_STAMP_OFFSET + 1;
mem_write_data <= mem_latch_data.stamp.nanosec;
mem_data_next.stamp <= mem_latch_data.stamp;
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
-- Synthesis Guard
if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then
mem_cnt_next <= mem_cnt + 1;
else
mem_cnt_next <= mem_cnt + 3;
end if;
end if;
-- SET Deadline 1/2
when 11 =>
-- Synthesis Guard
if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET;
mem_write_data <= ROS_TIME_INFINITE.sec;
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
end if;
-- SET Deadline 2/2
when 12 =>
-- Synthesis Guard
if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET + 1;
mem_write_data <= ROS_TIME_INFINITE.nanosec;
mem_data_next.deadline <= ROS_TIME_INFINITE;
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
end if;
-- SET Prev Addr
when 13 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_PREV_ADDR_OFFSET;
mem_write_data <= std_logic_vector(resize(GOAL_MEMORY_MAX_ADDRESS,WORD_WIDTH));
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- SET Next Addr
when 14 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET;
mem_write_data <= std_logic_vector(resize(mem_occupied_head,WORD_WIDTH));
mem_data_next.addr <= mem_addr_base;
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
-- Update Occupied Head
mem_occupied_head_next <= mem_addr_base;
-- Initial Occupied Tail
if (mem_occupied_tail = GOAL_MEMORY_MAX_ADDRESS) then
mem_occupied_tail_next <= mem_addr_base;
end if;
-- DONE
mem_stage_next <= IDLE;
end if;
when others =>
null;
end case;
when GET =>
case (mem_cnt) is
-- GET Goal State
when 0 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_STATE_OFFSET;
mem_read <= '1';
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
if check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then
mem_cnt_next <= 1;
elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then
mem_cnt_next <= 5;
elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
mem_cnt_next <= 7;
elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
mem_cnt_next <= 9;
else
mem_cnt_next <= 10;
end if;
end if;
-- GET Goal ID 1/4
when 1 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET;
mem_read <= '1';
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- GET Goal ID 2/4
when 2 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 1;
mem_read <= '1';
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- GET Goal ID 3/4
when 3 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 2;
mem_read <= '1';
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- GET Goal ID 4/4
when 4 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 3;
mem_read <= '1';
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
if check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then
mem_cnt_next <= 5;
elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
mem_cnt_next <= 7;
elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
mem_cnt_next <= 9;
else
if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then
mem_cnt_next <= 10;
else
mem_cnt_next <= 11;
end if;
end if;
end if;
-- GET Stamp 1/2
when 5 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_STAMP_OFFSET;
mem_read <= '1';
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- GET Stamp 2/2
when 6 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_STAMP_OFFSET + 1;
mem_read <= '1';
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
mem_cnt_next <= 7;
elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
mem_cnt_next <= 9;
else
if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then
mem_cnt_next <= 10;
elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then
mem_cnt_next <= 11;
else
mem_cnt_next <= 15;
end if;
end if;
end if;
-- GET Deadline 1/2
when 7 =>
-- Synthesis Guard
if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET;
mem_read <= '1';
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
end if;
-- GET Deadline 2/2
when 8 =>
-- Synthesis Guard
if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET + 1;
mem_read <= '1';
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
if check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
mem_cnt_next <= 9;
else
if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then
mem_cnt_next <= 10;
elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then
mem_cnt_next <= 11;
elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then
mem_cnt_next <= 15;
else
mem_cnt_next <= 17;
end if;
end if;
end if;
end if;
-- GET Result Index
when 9 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_RESULT_INDEX_OFFSET;
mem_read <= '1';
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then
mem_cnt_next <= 10;
elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then
mem_cnt_next <= 11;
elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then
mem_cnt_next <= 15;
elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
mem_cnt_next <= 17;
else
mem_cnt_next <= 19;
end if;
end if;
-- READ Goal State
when 10 =>
mem_ready_out <= '1';
-- Memory Flow Control Guard
if (mem_valid_out = '1') then
mem_data_next.state <= mem_read_data(CDR_INT8_WIDTH-1 downto 0);
if check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then
mem_cnt_next <= 11;
elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then
mem_cnt_next <= 15;
elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
mem_cnt_next <= 17;
elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
mem_cnt_next <= 19;
else
mem_stage_next <= IDLE;
end if;
end if;
-- READ Goal ID 1/4
when 11 =>
mem_ready_out <= '1';
-- Memory Flow Control Guard
if (mem_valid_out = '1') then
mem_data_next.goal_id(0) <= mem_read_data;
mem_cnt_next <= mem_cnt + 1;
end if;
-- READ Goal ID 2/4
when 12 =>
mem_ready_out <= '1';
-- Memory Flow Control Guard
if (mem_valid_out = '1') then
mem_data_next.goal_id(1) <= mem_read_data;
mem_cnt_next <= mem_cnt + 1;
end if;
-- READ Goal ID 3/4
when 13 =>
mem_ready_out <= '1';
-- Memory Flow Control Guard
if (mem_valid_out = '1') then
mem_data_next.goal_id(2) <= mem_read_data;
mem_cnt_next <= mem_cnt + 1;
end if;
-- READ Goal ID 4/4
when 14 =>
mem_ready_out <= '1';
-- Memory Flow Control Guard
if (mem_valid_out = '1') then
mem_data_next.goal_id(3) <= mem_read_data;
if check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then
mem_cnt_next <= 15;
elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
mem_cnt_next <= 17;
elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
mem_cnt_next <= 19;
else
mem_stage_next <= IDLE;
end if;
end if;
-- READ Stamp 1/2
when 15 =>
mem_ready_out <= '1';
-- Memory Flow Control Guard
if (mem_valid_out = '1') then
mem_data_next.stamp.sec <= mem_read_data;
mem_cnt_next <= mem_cnt + 1;
end if;
-- READ Stamp 2/2
when 16 =>
mem_ready_out <= '1';
-- Memory Flow Control Guard
if (mem_valid_out = '1') then
mem_data_next.stamp.nanosec <= mem_read_data;
if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
mem_cnt_next <= 17;
elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
mem_cnt_next <= 19;
else
mem_stage_next <= IDLE;
end if;
end if;
-- READ Deadline 1/2
when 17 =>
-- Synthesis Guard
if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then
mem_ready_out <= '1';
-- Memory Flow Control Guard
if (mem_valid_out = '1') then
mem_data_next.deadline.sec <= mem_read_data;
mem_cnt_next <= mem_cnt + 1;
end if;
end if;
-- READ Deadline 2/2
when 18 =>
-- Synthesis Guard
if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then
mem_ready_out <= '1';
-- Memory Flow Control Guard
if (mem_valid_out = '1') then
mem_data_next.deadline.nanosec <= mem_read_data;
if check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
mem_cnt_next <= 19;
else
mem_stage_next <= IDLE;
end if;
end if;
end if;
-- READ Result index
when 19 =>
mem_ready_out <= '1';
-- Memory Flow Control Guard
if (mem_valid_out = '1') then
mem_data_next.res_ind <= mem_read_data;
mem_stage_next <= IDLE;
end if;
when others =>
null;
end case;
when GET_NEXT =>
case (mem_cnt) is
-- GET Next Addr
when 0 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET;
mem_read <= '1';
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- READ Next Addr
when 1 =>
mem_ready_out <= '1';
-- Memory Flow Control Guard
if (mem_valid_out = '1') then
mem_addr_base_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH);
mem_stage_next <= GET;
if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then
mem_cnt_next <= 0;
elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then
mem_cnt_next <= 1;
elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then
mem_cnt_next <= 5;
elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
mem_cnt_next <= 7;
elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
mem_cnt_next <= 9;
else
-- DONE
mem_stage_next <= IDLE;
end if;
end if;
when others =>
null;
end case;
when GET_PREV =>
case (mem_cnt) is
-- GET Previous Addr
when 0 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_PREV_ADDR_OFFSET;
mem_read <= '1';
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- READ Previous Addr
when 1 =>
mem_ready_out <= '1';
-- Memory Flow Control Guard
if (mem_valid_out = '1') then
mem_addr_base_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH);
mem_stage_next <= GET;
if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then
mem_cnt_next <= 0;
elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then
mem_cnt_next <= 1;
elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then
mem_cnt_next <= 5;
elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
mem_cnt_next <= 7;
elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
mem_cnt_next <= 9;
else
-- DONE
mem_stage_next <= IDLE;
end if;
end if;
when others =>
null;
end case;
when UPDATE =>
case (mem_cnt) is
-- SET Goal State
when 0 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_STATE_OFFSET;
mem_write_data <= (others => '0');
mem_write_data(CDR_INT8_WIDTH-1 downto 0) <= mem_latch_data.state;
mem_data_next.state <= mem_latch_data.state;
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
if check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then
mem_cnt_next <= 1;
elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then
mem_cnt_next <= 5;
elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
mem_cnt_next <= 7;
elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
mem_cnt_next <= 9;
else
mem_stage_next <= IDLE;
end if;
end if;
-- SET Goal ID 1/4
when 1 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET;
mem_write_data <= mem_latch_data.goal_id(0);
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- SET Goal ID 2/4
when 2 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 1;
mem_write_data <= mem_latch_data.goal_id(1);
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- SET Goal ID 3/4
when 3 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 2;
mem_write_data <= mem_latch_data.goal_id(3);
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- SET Goal ID 4/4
when 4 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_GOAL_ID_OFFSET + 3;
mem_write_data <= mem_latch_data.goal_id(3);
mem_data_next.goal_id <= mem_latch_data.goal_id;
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
if check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then
mem_cnt_next <= 5;
elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
mem_cnt_next <= 7;
elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
mem_cnt_next <= 9;
else
mem_stage_next <= IDLE;
end if;
end if;
-- SET Stamp 1/2
when 5 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_STAMP_OFFSET;
mem_write_data <= mem_latch_data.stamp.sec;
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- SET Stamp 2/2
when 6 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_STAMP_OFFSET + 1;
mem_write_data <= mem_latch_data.stamp.nanosec;
mem_data_next.stamp <= mem_latch_data.stamp;
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
mem_cnt_next <= 7;
elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
mem_cnt_next <= 9;
else
mem_stage_next <= IDLE;
end if;
end if;
-- SET Deadline 1/2
when 7 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET;
mem_write_data <= mem_latch_data.deadline.sec;
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- SET Deadline 2/2
when 8 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_DEADLINE_OFFSET + 1;
mem_write_data <= mem_latch_data.deadline.sec;
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
if check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
mem_cnt_next <= 9;
else
mem_stage_next <= IDLE;
end if;
end if;
-- SET Result Index
when 9 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_RESULT_INDEX_OFFSET;
mem_write_data <= mem_latch_data.res_ind;
mem_data_next.res_ind <= mem_latch_data.res_ind;
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
-- DONE
mem_stage_next <= IDLE;
end if;
when others =>
null;
end case;
when REMOVE =>
case (mem_cnt) is
-- GET Next Addr
when 0 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET;
mem_read <= '1';
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- GET Previous Addr
when 1 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_PREV_ADDR_OFFSET;
mem_read <= '1';
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- SET Next Addr
when 2 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET;
mem_write_data <= std_logic_vector(resize(GOAL_MEMORY_MAX_ADDRESS,WORD_WIDTH));
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
-- Set New Empty Head
mem_empty_head_next <= mem_addr_base;
mem_cnt_next <= mem_cnt + 1;
end if;
-- SET Next Addr (Current Tail)
when 3 =>
mem_valid_in <= '1';
mem_addr <= mem_empty_tail + GMF_NEXT_ADDR_OFFSET;
mem_write_data <= std_logic_vector(resize(mem_addr_base,WORD_WIDTH));
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
-- Set New Empty TAIL
mem_empty_tail_next <= mem_addr_base;
mem_cnt_next <= mem_cnt + 1;
end if;
-- READ Next Addr
when 4 =>
mem_ready_out <= '1';
-- Memory Flow Control Guard
if (mem_valid_out = '1') then
mem_addr_latch_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH);
mem_cnt_next <= mem_cnt + 1;
end if;
-- READ Previous Addr
when 5 =>
mem_ready_out <= '1';
-- Memory Flow Control Guard
if (mem_valid_out = '1') then
-- Set mem_data.addr to previous slot
mem_data_next.addr <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH);
if (mem_addr_latch = GOAL_MEMORY_MAX_ADDRESS) then
-- Set New Occupied Tail
mem_occupied_tail_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH);
if (resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH) = GOAL_MEMORY_MAX_ADDRESS) then
-- Reset Occupied Head
mem_occupied_head_next <= GOAL_MEMORY_MAX_ADDRESS;
mem_stage_next <= IDLE;
else
mem_addr_base_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH);
mem_cnt_next <= mem_cnt + 2;
end if;
else
mem_addr_base_next <= mem_addr_latch;
mem_addr_latch_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH);
mem_cnt_next <= mem_cnt + 1;
end if;
end if;
-- SET Previous Addr (Next Slot)
when 6 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_PREV_ADDR_OFFSET;
mem_write_data <= std_logic_vector(resize(mem_addr_latch,WORD_WIDTH));
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
if (mem_addr_latch = GOAL_MEMORY_MAX_ADDRESS) then
-- Set New Occupied Head
mem_occupied_head_next <= mem_addr_base;
-- DONE
mem_stage_next <= IDLE;
else
mem_addr_base_next <= mem_addr_latch;
mem_addr_latch_next <= mem_addr_base;
mem_cnt_next <= mem_cnt + 1;
end if;
end if;
-- SET Next Addr (Previous Slot)
when 7 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET;
mem_write_data <= std_logic_vector(resize(mem_addr_latch,WORD_WIDTH));
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
-- DONE
mem_stage_next <= IDLE;
end if;
when others =>
null;
end case;
when RESET_MEMORY =>
case (mem_cnt) is
-- SET Result Index
when 0 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_RESULT_INDEX_OFFSET;
mem_write_data <= std_logic_vector(empty_head_res_ind);
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- SET Next Pointer
when 1 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_NEXT_ADDR_OFFSET;
if (mem_addr_base = MAX_GOAL_ADDRESS) then
mem_write_data <= std_logic_vector(resize(GOAL_MEMORY_MAX_ADDRESS,WORD_WIDTH));
else
mem_write_data <= std_logic_vector(resize(mem_addr_base + GOAL_FRAME_SIZE,WORD_WIDTH));
end if;
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
mem_cnt_next <= mem_cnt + 1;
end if;
-- SET Previous Pointer
when 2 =>
mem_valid_in <= '1';
mem_addr <= mem_addr_base + GMF_PREV_ADDR_OFFSET;
mem_write_data <= std_logic_vector(resize(mem_addr_latch,WORD_WIDTH));
-- Memory Flow Control Guard
if (mem_ready_in = '1') then
if (mem_addr_base = MAX_GOAL_ADDRESS) then
-- Initialize Empty and Occupied Heads
mem_empty_head_next <= FIRST_GOAL_ADDRESS;
mem_empty_tail_next <= MAX_GOAL_ADDRESS;
mem_occupied_head_next <= GOAL_MEMORY_MAX_ADDRESS;
mem_occupied_tail_next <= GOAL_MEMORY_MAX_ADDRESS;
empty_head_res_ind_next <= (others => '0');
-- DONE
mem_stage_next <= IDLE;
else
empty_head_res_ind_next <= empty_head_res_ind + 1;
mem_addr_latch_next <= mem_addr_base;
mem_addr_base_next <= mem_addr_base + GOAL_FRAME_SIZE;
mem_cnt_next <= 0;
end if;
end if;
when others =>
null;
end case;
end case;
end process;
sync_prc : process(clk)
begin
if rising_edge(clk) then
if (reset = '1') then
stage <= RESET_MEMORY;
goal_stage <= IDLE;
cancel_stage <= IDLE;
status_stage <= IDLE;
mem_stage <= RESET_MEMORY;
mem_data_r <= ZERO_GOAL_DATA;
mem_data_c <= ZERO_GOAL_DATA;
mem_data_s <= ZERO_GOAL_DATA;
mem_latch_data <= ZERO_GOAL_DATA;
mem_data <= ZERO_GOAL_DATA;
rrq_addr_base <= FIRST_RRQ_ADDRESS;
rrq_addr_latch <= RRQ_MEMORY_MAX_ADDRESS;
mem_addr_base <= FIRST_GOAL_ADDRESS;
mem_addr_latch <= GOAL_MEMORY_MAX_ADDRESS;
rrq_empty_head <= RRQ_MEMORY_MAX_ADDRESS;
rrq_occupied_head <= RRQ_MEMORY_MAX_ADDRESS;
mem_occupied_head <= GOAL_MEMORY_MAX_ADDRESS;
mem_occupied_tail <= GOAL_MEMORY_MAX_ADDRESS;
mem_empty_head <= GOAL_MEMORY_MAX_ADDRESS;
mem_empty_tail <= GOAL_MEMORY_MAX_ADDRESS;
return_code_latch <= ROS_RET_OK;
request_id_latch <= EMPTY_REQUEST_ID;
check_time <= ROS_TIME_INFINITE;
time_latch <= ROS_TIME_INVALID;
goal_handle_latch <= GOAL_HANDLE_UNKNOWN;
goal_state_latch <= GoalStatus_package.STATUS_UNKNOWN;
g_stamp <= ROS_TIME_INVALID;
search_type_c <= SEARCH_NONE;
cancel_ret_code <= CancelGoal_package.RR_ERROR_NONE;
cnt <= 0;
terminal_cnt <= 0;
c_cnt <= 0;
goals_canceling_cnt <= 0;
status_list_cnt <= 0;
mem_cnt <= 0;
trigger_result <= '0';
trigger_status <= '1'; -- Trigger Status Update immediately
g_accept <= '0';
empty_head_res_ind <= (others => '0');
else
stage <= stage_next;
goal_stage <= goal_stage_next;
cancel_stage <= cancel_stage_next;
status_stage <= status_stage_next;
mem_stage <= mem_stage_next;
mem_data_r <= mem_data_r_next;
mem_data_c <= mem_data_c_next;
mem_data_s <= mem_data_s_next;
mem_latch_data <= mem_latch_data_next;
mem_data <= mem_data_next;
rrq_addr_base <= rrq_addr_base_next;
rrq_addr_latch <= rrq_addr_latch_next;
mem_addr_base <= mem_addr_base_next;
mem_addr_latch <= mem_addr_latch_next;
rrq_empty_head <= rrq_empty_head_next;
rrq_occupied_head <= rrq_occupied_head_next;
mem_occupied_head <= mem_occupied_head_next;
mem_occupied_tail <= mem_occupied_tail_next;
mem_empty_head <= mem_empty_head_next;
mem_empty_tail <= mem_empty_tail_next;
return_code_latch <= return_code_latch_next;
request_id_latch <= request_id_latch_next;
check_time <= check_time_next;
time_latch <= time_latch_next;
goal_handle_latch <= goal_handle_latch_next;
goal_state_latch <= goal_state_latch_next;
g_stamp <= g_stamp_next;
search_type_c <= search_type_c_next;
cancel_ret_code <= cancel_ret_code_next;
cnt <= cnt_next;
terminal_cnt <= terminal_cnt_next;
c_cnt <= c_cnt_next;
goals_canceling_cnt <= goals_canceling_cnt_next;
status_list_cnt <= status_list_cnt_next;
mem_cnt <= mem_cnt_next;
trigger_result <= trigger_result_next;
trigger_status <= trigger_status_next;
g_accept <= g_accept_next;
empty_head_res_ind <= empty_head_res_ind_next;
end if;
end if;
end process;
end architecture;
Reference in New Issue View Git Blame Copy Permalink