Add ROS Publisher/Subscriber Templates
This commit is contained in:
parent
0a84093edd
commit
1e1a896613
262
src/ros2/TEMPLATE_pub.vhd
Normal file
262
src/ros2/TEMPLATE_pub.vhd
Normal file
@ -0,0 +1,262 @@
|
||||
-- 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;
|
||||
|
||||
entity TEMPLATE_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_out_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_in_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###
|
||||
-- TYPE SPECIFIC PORTS
|
||||
-- ###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 TEMPLATE_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 (TODO);
|
||||
-- ###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 DECLARATION
|
||||
-- ###GENERATED END###
|
||||
|
||||
|
||||
begin
|
||||
|
||||
-- ###GENERATED START###
|
||||
-- MEMORY INSTANTIATIONS
|
||||
-- ###GENERATED END###
|
||||
|
||||
-- PASSTHROUGH
|
||||
instance_handle_out_dds <= HANDLE_NIL;
|
||||
source_ts_dds <= TIME_INVALID;
|
||||
max_wait_dds <= DURATION_ZERO;
|
||||
ready_in_dds <= '0'; -- DDS Writer Input is unused
|
||||
-- ###GENERATED START###
|
||||
-- PORT SIGNAL CONNECTIONS
|
||||
-- ###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###
|
||||
-- DEFAULT SIGNAL ASSIGNMENTS
|
||||
-- ###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###
|
||||
-- MEMORY SIGNAL CONNECTIONS
|
||||
-- ###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 <= TODO;
|
||||
-- ###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 TODO =>
|
||||
-- ###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 <= TODO;
|
||||
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###
|
||||
-- RESET SYNC SIGNAL VALUE
|
||||
-- ###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###
|
||||
-- SYNC SIGNALS
|
||||
-- ###GENERATED END###
|
||||
end if;
|
||||
end if;
|
||||
end process;
|
||||
|
||||
end architecture;
|
||||
428
src/ros2/TEMPLATE_sub.vhd
Normal file
428
src/ros2/TEMPLATE_sub.vhd
Normal file
@ -0,0 +1,428 @@
|
||||
-- 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;
|
||||
|
||||
entity TEMPLATE_sub is
|
||||
port (
|
||||
-- SYSTEM
|
||||
clk : in std_logic;
|
||||
reset : in std_logic;
|
||||
-- FROM DDS READER
|
||||
start_dds : out std_logic;
|
||||
ack_dds : in std_logic;
|
||||
opcode_dds : out DDS_READER_OPCODE_TYPE;
|
||||
instance_state_dds : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
|
||||
view_state_dds : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
|
||||
sample_state_dds : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
|
||||
instance_handle_dds : out INSTANCE_HANDLE_TYPE;
|
||||
max_samples_dds : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0);
|
||||
get_data_dds : out std_logic;
|
||||
done_dds : in std_logic;
|
||||
return_code_dds : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
|
||||
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;
|
||||
-- Sample Info
|
||||
si_sample_state_dds : in std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
|
||||
si_view_state_dds : in std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
|
||||
si_instance_state_dds : in std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
|
||||
si_source_timestamp_dds : in TIME_TYPE;
|
||||
si_instance_handle_dds : in INSTANCE_HANDLE_TYPE;
|
||||
si_publication_handle_dds : in INSTANCE_HANDLE_TYPE;
|
||||
si_disposed_generation_count_dds : in std_logic_vector(DISPOSED_GENERATION_COUNT_WIDTH-1 downto 0);
|
||||
si_no_writers_generation_count_dds : in std_logic_vector(NO_WRITERS_GENERATION_COUNT_WIDTH-1 downto 0);
|
||||
si_sample_rank_dds : in std_logic_vector(SAMPLE_RANK_WIDTH-1 downto 0);
|
||||
si_generation_rank_dds : in std_logic_vector(GENERATION_RANK_WIDTH-1 downto 0);
|
||||
si_absolute_generation_rank_dds : in std_logic_vector(ABSOLUTE_GENERATION_COUNT_WIDTH-1 downto 0);
|
||||
si_valid_data_dds : in std_logic;
|
||||
si_valid_dds : in std_logic;
|
||||
si_ack_dds : out std_logic;
|
||||
eoc_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;
|
||||
|
||||
done_user : out std_logic;
|
||||
return_code_user : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
|
||||
|
||||
data_available_user : out std_logic;
|
||||
|
||||
-- ###GENERATED START###
|
||||
-- TYPE SPECIFIC PORTS
|
||||
-- ###GENERATED END###
|
||||
message_info_user : out MESSAGE_INFO_TYPE;
|
||||
taken_user : out std_logic
|
||||
);
|
||||
end entity;
|
||||
|
||||
architecture arch of TEMPLATE_sub is
|
||||
|
||||
--*****TYPE DECLARATION*****
|
||||
-- FSM states. Explained below in detail
|
||||
type STAGE_TYPE is (IDLE,INITIATE_READ,WAIT_FOR_READER,WAIT_FOR_DATA,GET_PAYLOAD_HEADER,FETCH,ALIGN_STREAM,SKIP_PAYLOAD,DECODE_PAYLOAD,RETURN_ROS);
|
||||
-- ###GENERATED START###
|
||||
type DECODE_STAGE_TYPE is (TODO,GET_OPTIONAL_HEADER);
|
||||
-- ###GENERATED END###
|
||||
|
||||
-- *MAIN PROCESS*
|
||||
signal stage, stage_next : STAGE_TYPE;
|
||||
signal cnt, cnt_next : natural range 0 to 5;
|
||||
signal endian_flag, endian_flag_next : std_logic;
|
||||
signal last_word_in_latch, last_word_in_latch_next : std_logic;
|
||||
signal decode_error_latch, decode_error_latch_next : std_logic;
|
||||
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_in_latch, data_in_latch_next : std_logic_vector(WORD_WIDTH-1 downto 0);
|
||||
signal optional, optional_next : std_logic;
|
||||
signal abort_mem : std_logic;
|
||||
signal ready_in_dds_sig : std_logic;
|
||||
signal taken_sig, taken_sig_next : std_logic;
|
||||
signal data_available_sig, data_available_sig_next : std_logic;
|
||||
signal message_info_sig, message_info_sig_next : MESSAGE_INFO_TYPE;
|
||||
signal return_code_latch, return_code_latch_next : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
|
||||
signal decode_stage, decode_stage_next : DECODE_STAGE_TYPE;
|
||||
signal return_stage, return_stage_next : DECODE_STAGE_TYPE;
|
||||
-- ###GENERATED START###
|
||||
-- SIGNAL DECLARATIONS
|
||||
-- ###GENERATED END###
|
||||
|
||||
--*****ALIAS DECLARATION*****
|
||||
alias representation_id : std_logic_vector(PAYLOAD_REPRESENTATION_ID_WIDTH-1 downto 0) is data_in_dds(WORD_WIDTH-1 downto WORD_WIDTH-PAYLOAD_REPRESENTATION_ID_WIDTH);
|
||||
alias representation_options : std_logic_vector(PAYLOAD_REPRESENTATION_ID_WIDTH-1 downto 0) is data_in_dds(PAYLOAD_REPRESENTATION_OPTIONS_WIDTH-1 downto 0);
|
||||
alias parameter_id : std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0) is data_in_latch(WORD_WIDTH-1 downto WORD_WIDTH-PARAMETER_ID_WIDTH);
|
||||
alias parameter_length : std_logic_vector(PARAMETER_LENGTH_WIDTH-1 downto 0) is data_in_latch(PARAMETER_LENGTH_WIDTH-1 downto 0);
|
||||
begin
|
||||
|
||||
-- ###GENERATED START###
|
||||
-- MEMORY INSTANTIATIONS
|
||||
-- ###GENERATED END###
|
||||
|
||||
-- PASSTHROUGH
|
||||
taken_user <= taken_sig;
|
||||
ready_in_dds <= ready_in_dds_sig;
|
||||
message_info_user <= message_info_sig;
|
||||
data_available_user <= data_available_sig;
|
||||
instance_state_dds <= ANY_INSTANCE_STATE;
|
||||
view_state_dds <= ANY_VIEW_STATE;
|
||||
sample_state_dds <= ANY_SAMPLE_STATE;
|
||||
instance_handle_dds <= HANDLE_NIL;
|
||||
max_samples_dds <= (others => '0');
|
||||
-- ###GENERATED START###
|
||||
-- PORT SIGNAL CONNECTIONS
|
||||
-- ###GENERATED END###
|
||||
|
||||
|
||||
main_prc : process (all)
|
||||
variable tmp_length : unsigned(WORD_WIDTH-1 downto 0);
|
||||
begin
|
||||
-- DEFAULT
|
||||
stage_next <= stage;
|
||||
decode_stage_next <= decode_stage;
|
||||
return_stage_next <= return_stage;
|
||||
cnt_next <= cnt;
|
||||
endian_flag_next <= endian_flag;
|
||||
last_word_in_latch_next <= last_word_in_latch;
|
||||
decode_error_latch_next <= decode_error_latch;
|
||||
align_offset_next <= align_offset;
|
||||
target_align_next <= target_align;
|
||||
optional_next <= optional;
|
||||
taken_sig_next <= taken_sig;
|
||||
data_in_latch_next <= data_in_latch;
|
||||
align_op_next <= align_op;
|
||||
data_available_sig_next <= data_available_sig;
|
||||
return_code_latch_next <= return_code_latch;
|
||||
message_info_sig_next <= message_info_sig;
|
||||
abort_mem <= '0';
|
||||
ready_in_dds_sig <= '0';
|
||||
si_ack_dds <= '0';
|
||||
get_data_dds <= '0';
|
||||
start_dds <= '0';
|
||||
opcode_dds <= NOP;
|
||||
ack_user <= '0';
|
||||
done_user <= '0';
|
||||
return_code_user <= ROS_RET_OK;
|
||||
-- ###GENERATED START###
|
||||
-- DEFAULT SIGNAL ASSIGNMENTS
|
||||
-- ###GENERATED END###
|
||||
|
||||
-- Last Word Latch Setter
|
||||
if (last_word_in_dds = '1') then
|
||||
last_word_in_latch_next <= '1';
|
||||
end if;
|
||||
-- Data Available Setter
|
||||
if (check_mask(status_dds, DATA_AVAILABLE_STATUS)) then
|
||||
data_available_sig_next <= '1';
|
||||
end if;
|
||||
|
||||
case (stage) is
|
||||
when IDLE =>
|
||||
if (start_user = '1') then
|
||||
ack_user <= '1';
|
||||
case (opcode_user) is
|
||||
when TAKE =>
|
||||
stage_next <= INITIATE_READ;
|
||||
when others =>
|
||||
return_code_latch_next <= ROS_RET_UNSUPPORTED;
|
||||
stage_next <= RETURN_ROS;
|
||||
end case;
|
||||
-- RESET
|
||||
taken_sig_next <= '0';
|
||||
abort_mem <= '1';
|
||||
else
|
||||
-- ###GENERATED START###
|
||||
-- MEMORY SIGNAL CONNECTIONS
|
||||
-- ###GENERATED END###
|
||||
end if;
|
||||
when RETURN_ROS =>
|
||||
done_user <= '1';
|
||||
return_code_user <= return_code_latch;
|
||||
|
||||
-- DONE
|
||||
stage_next <= IDLE;
|
||||
when INITIATE_READ =>
|
||||
start_dds <= '1';
|
||||
opcode_dds <= TAKE_NEXT_SAMPLE;
|
||||
|
||||
if (ack_dds = '1') then
|
||||
stage_next <= WAIT_FOR_READER;
|
||||
end if;
|
||||
when WAIT_FOR_READER =>
|
||||
if (done_dds = '1') then
|
||||
case (return_code_dds) is
|
||||
when RETCODE_OK =>
|
||||
stage_next <= WAIT_FOR_DATA;
|
||||
when RETCODE_NO_DATA =>
|
||||
assert (taken_sig = '0') severity FAILURE;
|
||||
|
||||
-- Data Available Resetter
|
||||
data_available_sig_next <= '0';
|
||||
|
||||
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;
|
||||
when WAIT_FOR_DATA =>
|
||||
if (si_valid_dds = '1') then
|
||||
si_ack_dds <= '1';
|
||||
-- Meta Sample
|
||||
if (si_valid_data_dds = '0') then
|
||||
-- Ignore and read Next Sample
|
||||
stage_next <= INITIATE_READ;
|
||||
else
|
||||
get_data_dds <= '1';
|
||||
stage_next <= GET_PAYLOAD_HEADER;
|
||||
|
||||
message_info_sig_next.publisher_gid <= to_gid(GUID_UNKNOWN);
|
||||
message_info_sig_next.received_timestamp <= TIME_INVALID;
|
||||
message_info_sig_next.source_timestamp <= si_source_timestamp_dds;
|
||||
end if;
|
||||
end if;
|
||||
when GET_PAYLOAD_HEADER =>
|
||||
-- TODO: Latch Offset from Options Field?
|
||||
|
||||
ready_in_dds_sig <= '1';
|
||||
-- Input Guard
|
||||
if (valid_in_dds = '1') then
|
||||
case (representation_id) is
|
||||
when CDR_BE =>
|
||||
endian_flag_next <= '0';
|
||||
stage_next <= FETCH;
|
||||
-- Alignment Reset
|
||||
align_offset_next <= (others => '0');
|
||||
-- ###GENERATED START###
|
||||
decode_stage_next <= TODO;
|
||||
-- ###GENERATED END###
|
||||
-- Initial Fetch
|
||||
when CDR_LE =>
|
||||
endian_flag_next <= '1';
|
||||
stage_next <= FETCH;
|
||||
-- Alignment Reset
|
||||
align_offset_next <= (others => '0');
|
||||
-- ###GENERATED START###
|
||||
decode_stage_next <= TODO;
|
||||
-- ###GENERATED END###
|
||||
when others =>
|
||||
-- Unknown Payload Encoding
|
||||
stage_next <= SKIP_PAYLOAD;
|
||||
decode_error_latch_next <= '1';
|
||||
end case;
|
||||
end if;
|
||||
when FETCH =>
|
||||
ready_in_dds_sig <= '1';
|
||||
-- Input Guard
|
||||
if (valid_in_dds = '1') then
|
||||
data_in_latch_next <= data_in_dds;
|
||||
-- Alignment Operation in progress
|
||||
if (align_op = '1') then
|
||||
stage_next <= ALIGN_STREAM;
|
||||
-- Reset
|
||||
align_op_next <= '0';
|
||||
else
|
||||
stage_next <= DECODE_PAYLOAD;
|
||||
end if;
|
||||
end if;
|
||||
when ALIGN_STREAM =>
|
||||
-- Target Stream Alignment reached
|
||||
if (check_align(align_offset, target_align)) then
|
||||
-- DONE
|
||||
stage_next <= DECODE_PAYLOAD;
|
||||
else
|
||||
align_offset_next <= align_offset + 1;
|
||||
-- Need to fetch new Input Word
|
||||
if (align_offset(1 downto 0) = "11") then
|
||||
align_op_next <= '1';
|
||||
stage_next <= FETCH;
|
||||
end if;
|
||||
end if;
|
||||
when DECODE_PAYLOAD =>
|
||||
case (decode_stage) is
|
||||
-- ###GENERATED START###
|
||||
when TODO =>
|
||||
-- ###GENERATED END###
|
||||
when GET_OPTIONAL_HEADER =>
|
||||
-- ALIGN GUARD
|
||||
if (not check_align(align_offset, ALIGN_4)) then
|
||||
target_align_next <= ALIGN_4;
|
||||
stage_next <= ALIGN_STREAM;
|
||||
else
|
||||
case (cnt) is
|
||||
-- Optional Member Header
|
||||
when 0 =>
|
||||
-- Extended Parameter Header
|
||||
if (endian_swap(endian_flag,parameter_id) = PID_EXTENDED) then
|
||||
cnt_next <= cnt + 1;
|
||||
stage_next <= FETCH;
|
||||
else
|
||||
stage_next <= FETCH;
|
||||
decode_stage_next <= return_stage;
|
||||
cnt_next <= 0;
|
||||
-- Alignment Reset
|
||||
align_offset_next <= (others => '0');
|
||||
|
||||
-- Optional omitted
|
||||
if(endian_swap(endian_flag,parameter_length) = (parameter_length'reverse_range => '0')) then
|
||||
optional_next <= '0';
|
||||
else
|
||||
optional_next <= '1';
|
||||
end if;
|
||||
end if;
|
||||
-- eMemberHeader
|
||||
when 1 =>
|
||||
-- Ignore Parameter ID
|
||||
cnt_next <= cnt + 1;
|
||||
stage_next <= FETCH;
|
||||
-- Llength
|
||||
when 2 =>
|
||||
stage_next <= FETCH;
|
||||
decode_stage_next <= return_stage;
|
||||
cnt_next <= 0;
|
||||
-- Alignment Reset
|
||||
align_offset_next <= (others => '0');
|
||||
|
||||
-- Optional omitted
|
||||
if(endian_swap(endian_flag, data_in_dds) = (data_in_dds'reverse_range => '0')) then
|
||||
optional_next <= '0';
|
||||
else
|
||||
optional_next <= '1';
|
||||
end if;
|
||||
when others =>
|
||||
null;
|
||||
end case;
|
||||
end if;
|
||||
when others =>
|
||||
null;
|
||||
end case;
|
||||
when SKIP_PAYLOAD =>
|
||||
if (last_word_in_latch = '0') then
|
||||
-- Skip Read
|
||||
ready_in_dds_sig <= '1';
|
||||
else
|
||||
-- Reset
|
||||
last_word_in_latch_next <= '0';
|
||||
|
||||
-- If no Decode Error, mark output as valid
|
||||
if (decode_error_latch = '0') then
|
||||
taken_sig_next <= '1';
|
||||
return_code_latch_next <= ROS_RET_OK;
|
||||
else
|
||||
taken_sig_next <= '0';
|
||||
return_code_latch_next <= ROS_RET_ERROR;
|
||||
end if;
|
||||
|
||||
stage_next <= RETURN_ROS;
|
||||
end if;
|
||||
end case;
|
||||
|
||||
-- OVERREAD GUARD
|
||||
-- Attempted read on empty input
|
||||
if (last_word_in_latch = '1' and last_word_in_dds = '0' and ready_in_dds_sig = '1') then
|
||||
stage_next <= SKIP_PAYLOAD;
|
||||
decode_error_latch_next <= '1';
|
||||
end if;
|
||||
|
||||
end process;
|
||||
|
||||
sync_prc : process(clk)
|
||||
begin
|
||||
if rising_edge(clk) then
|
||||
if (reset = '1') then
|
||||
stage <= IDLE;
|
||||
decode_stage <= TODO;
|
||||
return_stage <= TODO;
|
||||
target_align <= ALIGN_1;
|
||||
return_code_latch <= ROS_RET_OK;
|
||||
message_info_sig <= EMPTY_MESSAGE_INFO;
|
||||
cnt <= 0;
|
||||
endian_flag <= '0';
|
||||
last_word_in_latch <= '0';
|
||||
decode_error_latch <= '0';
|
||||
optional <= '0';
|
||||
taken_sig <= '0';
|
||||
align_op <= '0';
|
||||
data_available_sig <= '0';
|
||||
align_offset <= (others => '0');
|
||||
data_in_latch <= (others => '0');
|
||||
-- ###GENERATED START###
|
||||
-- RESET SYNC SIGNAL VALUE
|
||||
-- ###GENERATED END###
|
||||
else
|
||||
stage <= stage_next;
|
||||
decode_stage <= decode_stage_next;
|
||||
return_stage <= return_stage_next;
|
||||
target_align <= target_align_next;
|
||||
return_code_latch <= return_code_latch_next;
|
||||
message_info_sig <= message_info_sig_next;
|
||||
cnt <= cnt_next;
|
||||
endian_flag <= endian_flag_next;
|
||||
last_word_in_latch <= last_word_in_latch_next;
|
||||
decode_error_latch <= decode_error_latch_next;
|
||||
optional <= optional_next;
|
||||
taken_sig <= taken_sig_next;
|
||||
align_op <= align_op_next;
|
||||
data_available_sig <= data_available_sig_next;
|
||||
align_offset <= align_offset_next;
|
||||
data_in_latch <= data_in_latch_next;
|
||||
-- ###GENERATED START###
|
||||
-- SYNC SIGNALS
|
||||
-- ###GENERATED END###
|
||||
end if;
|
||||
end if;
|
||||
end process;
|
||||
|
||||
end architecture;
|
||||
Loading…
Reference in New Issue
Block a user