390 lines
18 KiB
VHDL
390 lines
18 KiB
VHDL
-- 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.rtps_package.all;
|
|
use work.rtps_config_package.all;
|
|
use work.ros_package.all;
|
|
use work.Fibonacci_package.all;
|
|
|
|
entity Fibonacci_ros_action_feedback_pub is
|
|
generic (
|
|
LITTLE_ENDIAN : std_logic := '0'
|
|
);
|
|
port (
|
|
-- SYSTEM
|
|
clk : in std_logic;
|
|
reset : in std_logic;
|
|
-- FROM DDS WRITER
|
|
start_dds : out std_logic;
|
|
ack_dds : in std_logic;
|
|
opcode_dds : out DDS_WRITER_OPCODE_TYPE;
|
|
instance_handle_in_dds : out INSTANCE_HANDLE_TYPE;
|
|
source_ts_dds : out TIME_TYPE;
|
|
max_wait_dds : out DURATION_TYPE;
|
|
done_dds : in std_logic;
|
|
return_code_dds : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
|
|
instance_handle_out_dds : in INSTANCE_HANDLE_TYPE;
|
|
valid_out_dds : out std_logic;
|
|
ready_out_dds : in std_logic;
|
|
data_out_dds : out std_logic_vector(WORD_WIDTH-1 downto 0);
|
|
last_word_out_dds : out std_logic;
|
|
valid_in_dds : in std_logic;
|
|
ready_in_dds : out std_logic;
|
|
data_in_dds : in std_logic_vector(WORD_WIDTH-1 downto 0);
|
|
last_word_in_dds : in std_logic;
|
|
-- Communication Status
|
|
status_dds : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
|
|
|
|
-- TO USER ENTITY
|
|
start_user : in std_logic;
|
|
opcode_user : in ROS_TOPIC_OPCODE_TYPE;
|
|
ack_user : out std_logic;
|
|
|
|
-- ###GENERATED START###
|
|
goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0);
|
|
seq_len : in std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0);
|
|
seq_addr : in std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0);
|
|
seq_ready : out std_logic;
|
|
seq_ren : in std_logic;
|
|
seq_wen : in std_logic;
|
|
seq_valid : out std_logic;
|
|
seq_ack : in std_logic;
|
|
seq_r : out std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
|
|
seq_w : in std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
|
|
-- ###GENERATED END###
|
|
|
|
done_user : out std_logic;
|
|
return_code_user : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0)
|
|
);
|
|
end entity;
|
|
|
|
architecture arch of Fibonacci_ros_action_feedback_pub is
|
|
|
|
--*****TYPE DECLARATION*****
|
|
-- FSM states. Explained below in detail
|
|
type STAGE_TYPE is (IDLE,INITIATE_WRITE,WRITE_PAYLOAD_HEADER,PUSH,ALIGN_STREAM,ENCODE_PAYLOAD,WAIT_FOR_WRITER,RETURN_ROS);
|
|
-- ###GENERATED START###
|
|
type ENCODE_STAGE_TYPE is (WRITE_GOAL_ID, WRITE_SEQ_LENGTH, WRITE_SEQ);
|
|
-- ###GENERATED END###
|
|
|
|
-- *MAIN PROCESS*
|
|
signal stage, stage_next : STAGE_TYPE;
|
|
signal cnt, cnt_next : natural range 0 to 5;
|
|
signal align_offset, align_offset_next : unsigned(MAX_ALIGN_OFFSET_WIDTH-1 downto 0);
|
|
signal align_op, align_op_next : std_logic;
|
|
signal target_align, target_align_next : ALIGN_TYPE;
|
|
signal data_out_latch, data_out_latch_next : std_logic_vector(WORD_WIDTH-1 downto 0);
|
|
signal abort_mem : std_logic;
|
|
signal finalize_payload, finalize_payload_next : std_logic;
|
|
signal return_code_latch, return_code_latch_next : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
|
|
signal encode_stage, encode_stage_next : ENCODE_STAGE_TYPE;
|
|
-- ###GENERATED START###
|
|
signal uuid_cnt, uuid_cnt_next : natural range 0 to F_GOAL_ID_MAX_DEPTH-1;
|
|
signal seq_cnt, seq_cnt_next : natural range 0 to F_SEQ_MAX_DEPTH-1;
|
|
-- seq_mem SIGNALS
|
|
signal seq_mem_addr : std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0);
|
|
signal seq_mem_read, seq_mem_ready_in, seq_mem_ready_out, seq_mem_valid_in, seq_mem_valid_out : std_logic;
|
|
signal seq_mem_data_in, seq_mem_data_out : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
|
|
-- ###GENERATED END###
|
|
|
|
|
|
begin
|
|
|
|
-- ###GENERATED START###
|
|
seq_mem : entity work.mem_ctrl(arch)
|
|
generic map (
|
|
ADDR_WIDTH => F_SEQ_ADDR_WIDTH,
|
|
DATA_WIDTH => CDR_LONG_WIDTH,
|
|
MEMORY_DEPTH => F_SEQ_MAX_DEPTH,
|
|
MAX_BURST_LENGTH => 1
|
|
)
|
|
port map (
|
|
clk => clk,
|
|
reset => reset or abort_mem,
|
|
addr => seq_mem_addr,
|
|
read => seq_mem_read,
|
|
ready_in => seq_mem_ready_in,
|
|
valid_in => seq_mem_valid_in,
|
|
data_in => seq_mem_data_in,
|
|
ready_out => seq_mem_ready_out,
|
|
valid_out => seq_mem_valid_out,
|
|
data_out => seq_mem_data_out
|
|
);
|
|
-- ###GENERATED END###
|
|
|
|
-- PASSTHROUGH
|
|
instance_handle_in_dds <= HANDLE_NIL;
|
|
source_ts_dds <= TIME_INVALID;
|
|
max_wait_dds <= DURATION_ZERO;
|
|
ready_in_dds <= '0'; -- DDS Writer Input is unused
|
|
-- ###GENERATED START###
|
|
seq_valid <= seq_mem_valid_out;
|
|
seq_r <= seq_mem_data_out;
|
|
seq_mem_data_in <= seq_w;
|
|
-- ###GENERATED END###
|
|
|
|
main_prc : process (all)
|
|
begin
|
|
-- DEFAULT
|
|
stage_next <= stage;
|
|
encode_stage_next <= encode_stage;
|
|
cnt_next <= cnt;
|
|
align_offset_next <= align_offset;
|
|
align_op_next <= align_op;
|
|
target_align_next <= target_align;
|
|
data_out_latch_next <= data_out_latch;
|
|
finalize_payload_next <= finalize_payload;
|
|
return_code_latch_next <= return_code_latch;
|
|
abort_mem <= '0';
|
|
start_dds <= '0';
|
|
opcode_dds <= NOP;
|
|
valid_out_dds <= '0';
|
|
last_word_out_dds <= '0';
|
|
ack_user <= '0';
|
|
done_user <= '0';
|
|
return_code_user <= ROS_RET_OK;
|
|
data_out_dds <= (others => '0');
|
|
-- ###GENERATED START###
|
|
uuid_cnt_next <= uuid_cnt;
|
|
seq_cnt_next <= seq_cnt;
|
|
seq_mem_addr <= (others => '0');
|
|
seq_mem_read <= '0';
|
|
seq_mem_valid_in <= '0';
|
|
seq_mem_ready_out <= '0';
|
|
seq_ready <= '0';
|
|
-- ###GENERATED END###
|
|
|
|
case (stage) is
|
|
when IDLE =>
|
|
if (start_user = '1') then
|
|
ack_user <= '1';
|
|
case (opcode_user) is
|
|
when PUBLISH =>
|
|
stage_next <= INITIATE_WRITE;
|
|
when others =>
|
|
return_code_latch_next <= ROS_RET_UNSUPPORTED;
|
|
stage_next <= RETURN_ROS;
|
|
end case;
|
|
-- RESET
|
|
abort_mem <= '1';
|
|
else
|
|
-- ###GENERATED START###
|
|
seq_ready <= seq_mem_ready_in;
|
|
seq_mem_addr <= seq_addr;
|
|
seq_mem_read <= seq_ren;
|
|
seq_mem_valid_in <= seq_ren or seq_wen;
|
|
seq_mem_ready_out <= seq_ack;
|
|
-- ###GENERATED END###
|
|
end if;
|
|
when RETURN_ROS =>
|
|
done_user <= '1';
|
|
return_code_user <= return_code_latch;
|
|
|
|
-- DONE
|
|
stage_next <= IDLE;
|
|
when INITIATE_WRITE =>
|
|
start_dds <= '1';
|
|
opcode_dds <= WRITE;
|
|
|
|
if (ack_dds = '1') then
|
|
stage_next <= WRITE_PAYLOAD_HEADER;
|
|
end if;
|
|
|
|
when WRITE_PAYLOAD_HEADER =>
|
|
valid_out_dds <= '1';
|
|
if (LITTLE_ENDIAN = '0') then
|
|
data_out_dds <= CDR_BE & x"0000";
|
|
else
|
|
data_out_dds <= CDR_LE & x"0000";
|
|
end if;
|
|
-- Output Guard
|
|
if (ready_out_dds = '1') then
|
|
stage_next <= ENCODE_PAYLOAD;
|
|
-- Reset
|
|
align_offset_next <= (others => '0');
|
|
data_out_latch_next <= (others => '0');
|
|
-- ###GENERATED START###
|
|
encode_stage_next <= WRITE_GOAL_ID;
|
|
-- ###GENERATED END###
|
|
end if;
|
|
when PUSH =>
|
|
-- Mark Last Word
|
|
if (finalize_payload = '1') then
|
|
last_word_out_dds <= '1';
|
|
end if;
|
|
|
|
valid_out_dds <= '1';
|
|
data_out_dds <= data_out_latch;
|
|
-- Output Guard
|
|
if (ready_out_dds = '1') then
|
|
-- NOTE: Ensures all padding is zero.
|
|
data_out_latch_next <= (others => '0');
|
|
-- Alignment Operation in process
|
|
if (align_op = '1') then
|
|
stage_next <= ALIGN_STREAM;
|
|
-- Reset
|
|
align_op_next <= '0';
|
|
-- DONE
|
|
elsif (finalize_payload = '1') then
|
|
finalize_payload_next <= '0';
|
|
stage_next <= WAIT_FOR_WRITER;
|
|
else
|
|
stage_next <= ENCODE_PAYLOAD;
|
|
end if;
|
|
end if;
|
|
when ALIGN_STREAM =>
|
|
-- Target Stream Alignment reached
|
|
if (check_align(align_offset, target_align)) then
|
|
-- DONE
|
|
stage_next <= ENCODE_PAYLOAD;
|
|
else
|
|
align_offset_next <= align_offset + 1;
|
|
-- Need to push Word
|
|
if (align_offset(1 downto 0) = "11") then
|
|
align_op_next <= '1';
|
|
stage_next <= PUSH;
|
|
end if;
|
|
end if;
|
|
when ENCODE_PAYLOAD =>
|
|
case (encode_stage) is
|
|
-- ###GENERATED START###
|
|
when WRITE_GOAL_ID =>
|
|
-- Special Encoding for efficiency (Prevent having to define memory for UUID)
|
|
-- ALIGN GUARD
|
|
if (not check_align(align_offset, ALIGN_1)) then
|
|
target_align_next <= ALIGN_1;
|
|
stage_next <= ALIGN_STREAM;
|
|
else
|
|
data_out_latch_next <= write_sub_vector(data_out_latch, get_sub_vector(goal_id,uuid_cnt,CDR_INT8_WIDTH,TRUE), to_integer(align_offset(1 downto 0)), TRUE);
|
|
align_offset_next <= align_offset + 1;
|
|
|
|
if (uuid_cnt = F_GOAL_ID_MAX_DEPTH-1) then
|
|
encode_stage_next <= WRITE_SEQ_LENGTH;
|
|
cnt_next <= 0;
|
|
uuid_cnt_next <= 0; -- Post-Reset
|
|
else
|
|
uuid_cnt_next <= uuid_cnt + 1;
|
|
end if;
|
|
|
|
-- Need to fetch next Word
|
|
if(align_offset(1 downto 0) = "11") then
|
|
stage_next <= PUSH;
|
|
end if;
|
|
end if;
|
|
when WRITE_SEQ_LENGTH =>
|
|
-- ALIGN GUARD
|
|
if (not check_align(align_offset, ALIGN_4)) then
|
|
target_align_next <= ALIGN_4;
|
|
stage_next <= ALIGN_STREAM;
|
|
else
|
|
data_out_latch_next <= endian_swap(LITTLE_ENDIAN, std_logic_vector(resize(unsigned(seq_len),WORD_WIDTH)));
|
|
align_offset_next <= align_offset + 4;
|
|
stage_next <= PUSH;
|
|
|
|
-- Empty Sequence
|
|
if (unsigned(seq_len) = 0) then
|
|
-- DONE
|
|
stage_next <= PUSH;
|
|
finalize_payload_next <= '1';
|
|
else
|
|
encode_stage_next <= WRITE_SEQ;
|
|
cnt_next <= 0;
|
|
seq_cnt_next <= 0;
|
|
end if;
|
|
end if;
|
|
when WRITE_SEQ =>
|
|
-- ALIGN GUARD
|
|
if (not check_align(align_offset, ALIGN_4)) then
|
|
target_align_next <= ALIGN_4;
|
|
stage_next <= ALIGN_STREAM;
|
|
else
|
|
case (cnt) is
|
|
-- GET
|
|
when 0 =>
|
|
seq_mem_addr <= std_logic_vector(to_unsigned(seq_cnt,F_SEQ_ADDR_WIDTH));
|
|
seq_mem_valid_in <= '1';
|
|
seq_mem_read <= '1';
|
|
-- Memory Operation Guard
|
|
if (seq_mem_ready_in = '1') then
|
|
cnt_next <= cnt + 1;
|
|
end if;
|
|
-- WRITE
|
|
when 1 =>
|
|
seq_mem_ready_out <= '1';
|
|
-- Memory Operation Guard
|
|
if (seq_mem_valid_out = '1') then
|
|
data_out_latch_next <= endian_swap(LITTLE_ENDIAN, seq_mem_data_out);
|
|
align_offset_next <= align_offset + 4;
|
|
stage_next <= PUSH;
|
|
cnt_next <= 0;
|
|
|
|
-- All Elements processed
|
|
if (seq_cnt = unsigned(seq_len)-1) then
|
|
-- DONE
|
|
stage_next <= PUSH;
|
|
finalize_payload_next <= '1';
|
|
else
|
|
seq_cnt_next <= seq_cnt + 1;
|
|
end if;
|
|
end if;
|
|
when others =>
|
|
end case;
|
|
end if;
|
|
-- ###GENERATED END###
|
|
when others =>
|
|
null;
|
|
end case;
|
|
when WAIT_FOR_WRITER =>
|
|
if (done_dds = '1') then
|
|
case (return_code_dds) is
|
|
when RETCODE_OK =>
|
|
return_code_latch_next <= ROS_RET_OK;
|
|
stage_next <= RETURN_ROS;
|
|
when others =>
|
|
return_code_latch_next <= ROS_RET_ERROR;
|
|
stage_next <= RETURN_ROS;
|
|
end case;
|
|
end if;
|
|
end case;
|
|
end process;
|
|
|
|
sync_prc : process(clk)
|
|
begin
|
|
if rising_edge(clk) then
|
|
if (reset = '1') then
|
|
stage <= IDLE;
|
|
encode_stage <= WRITE_GOAL_ID;
|
|
target_align <= ALIGN_1;
|
|
return_code_latch <= ROS_RET_OK;
|
|
cnt <= 0;
|
|
finalize_payload <= '0';
|
|
align_op <= '0';
|
|
align_offset <= (others => '0');
|
|
data_out_latch <= (others => '0');
|
|
-- ###GENERATED START###
|
|
uuid_cnt <= 0;
|
|
seq_cnt <= 0;
|
|
-- ###GENERATED END###
|
|
else
|
|
stage <= stage_next;
|
|
encode_stage <= encode_stage_next;
|
|
target_align <= target_align_next;
|
|
return_code_latch <= return_code_latch_next;
|
|
cnt <= cnt_next;
|
|
finalize_payload <= finalize_payload_next;
|
|
align_op <= align_op_next;
|
|
align_offset <= align_offset_next;
|
|
data_out_latch <= data_out_latch_next;
|
|
-- ###GENERATED START###
|
|
uuid_cnt <= uuid_cnt_next;
|
|
seq_cnt <= seq_cnt_next;
|
|
-- ###GENERATED END###
|
|
end if;
|
|
end if;
|
|
end process;
|
|
|
|
end architecture; |