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.user_config.all;
use work.rtps_config_package.all;

entity rtps_handler is
    port (
        clk             : in std_logic;                                     -- Input Clock
        reset           : in std_logic;                                     -- Synchronous Reset
        empty           : in std_logic;                                     -- Input FIFO empty flag
        rd              : out std_logic;                                    -- Input FIFO read signal
        data_in         : in std_logic_vector(WORD_WIDTH-1 downto 0);       -- Input FIFO data signal
        data_out        : out std_logic_vector(WORD_WIDTH-1 downto 0);      -- Output data signal
        builtin_full    : in std_logic;                                     -- Output FIFO (Built-In Endpoint) full signal
        builtin_wr      : out std_logic;                                    -- Output FIFO (Built-In Endpoint) write signal
        user_full       : in std_logic_vector(0 to NUM_ENDPOINTS-1);        -- Output FIFO (User Endpoints) full signal
        user_wr         : out std_logic_vector(0 to NUM_ENDPOINTS-1);       -- Output FIFO (User Endpoints) write signal
        last_word_out   : out std_logic                                     -- Output FIFO Last Word signal
    );
end entity;

architecture arch of rtps_handler is
    
    --*****COMPONENT DECLARATION******
    
    --*****CONSTANT DECLARATION*****
    
    --*****TYPE DECLARATION*****
    -- FSM states. Explained below in detail
    type STAGE_TYPE is (SRC_ADDR_HEADER, DEST_ADDR_HEADER, UDP_PORT_HEADER, LEN_HEADER, RTPS_HEADER_1, RTPS_HEADER_2,
        RTPS_HEADER_3, RTPS_SUB_HEADER, PARSE_INFO_DST, PARSE_INFO_SRC, PARSE_INFO_TS, PARSE_INFO_REPLY, EXTRACT_LOCATOR, 
        PARSE_INFO_REPLY_IP4, PARSE_HEARTBEAT, PARSE_ACKNACK, PARSE_GAP, PARSE_DATA, SKIP_DATA_HEADER, MATCH_DST_ENDPOINT, 
        PUSH_PAYLOAD_HEADER, PUSH_PAYLOAD, SKIP_SUB, SKIP_PACKET);
    type IPv4_ADDRESS_TYPE is (DEFAULT_META_ADDR, DEFAULT_ADDR, INVALID);
    
    
    --*****SIGNAL DECLARATION*****
    -- FSM state
    signal stage, stage_next : STAGE_TYPE := SRC_ADDR_HEADER;
    -- Intermediate input read signal. (Read from output port not allowed)
    signal rd_sig : std_logic := '0';
    -- Signal used to reset the 4-Byte Word counter
    signal reset_read_cnt : std_logic;
    -- 4-Byte Word counter (Counts words read from input FIFO)
    signal read_cnt : unsigned(UDP_HEADER_LENGTH_WIDTH-3 downto 0) := (others => '0');
    -- read_cnt + 1
    -- NOTE: Because the Submessage Length does not include the Submessage Header, we need a to add +1 to find the end of the Submessage
    --       In order to prevent two serial additions in the same clock cycle, we use this pre-incremented signal instead
    signal read_cnt_plus : unsigned(UDP_HEADER_LENGTH_WIDTH-3 downto 0) := (others => '0');
    -- Total packet length (4-Byte Words)
    signal packet_length, packet_length_next : unsigned(UDP_HEADER_LENGTH_WIDTH-3 downto 0) := (others => '0');
    -- End of Submessage from the beginning of the UDP Packet in 4-Byte Words
    -- NOTE: We count this in 4-Byte Words, because Submessages always begin in a 4-Byte boundary (DDSI-RTPS 2.3 Section 9.4.1), 
    --       and thus the "sub_end" is always a multiple of four.
    signal sub_end, sub_end_next : unsigned(SUBMESSAGE_LENGTH_WIDTH-3 downto 0) := (others => '0');
    -- End of DATA Submessage Sub-Header (Beginning of inlineQoS/Payload) from the beginning of the UDP Packet in Bytes
    signal data_header_end, data_header_end_next : unsigned(SUBMESSAGE_LENGTH_WIDTH-1 downto 0) := (others => '0');
    -- Input Signal Latch. Used to read 4-Byte aligned from input (see align_prc)
    signal align_sig, align_sig_next : std_logic_vector((3*BYTE_WIDTH)-1 downto 0) := (others => '0');
    -- 4-Byte Aligned Input (see align_prc)
    signal data_in_aligned : std_logic_vector(WORD_WIDTH-1 downto 0);
    -- 4-Byte Alignement offset (see align_prc)
    signal align_offset, align_offset_next : std_logic_vector(1 downto 0) := (others => '0');
    -- 4-Byte Alignement offset latch
    signal offset_latch, offset_latch_next : std_logic_vector(1 downto 0) := (others => '0');
    -- IPv4 Source Address latch
    signal src_addr, src_addr_next : std_logic_vector(IPv4_ADDRESS_WIDTH-1 downto 0) := (others => '0');
    -- UDP Source Port latch
    signal src_port, src_port_next : std_logic_vector(UDP_PORT_WIDTH-1 downto 0) := (others => '0');
    -- Denotes if processed Message is Metatraffic
    signal is_metatraffic, is_metatraffic_next : std_logic := '0';
    -- RTPS Submessage Flag latch
    signal flags, flags_next : std_logic_vector(SUBMESSAGE_FLAGS_WIDTH-1 downto 0) := (others => '0');
    -- Source Entity ID latch
    signal src_entityid, src_entityid_next  : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0');
    -- Destination Entity ID latch
    signal dest_entityid, dest_entityid_next  : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0');
    -- Source GUID Prefix latch
    signal src_guidprefix, src_guidprefix_next : GUIDPREFIX_TYPE := (others => (others => '0'));
    -- Vector denoting the Destination User Endpoints of the Submessage
    signal user_endpoint, user_endpoint_next : std_logic_vector(0 to NUM_ENDPOINTS-1) := (others => '0');
    -- Denotes if the Message is destined for the Built-in Endpoints
    signal builtin_endpoint, builtin_endpoint_next : std_logic := '0';
    -- LocatorList NumLocator latch
    -- NOTE: Since Submessages are limited to 2^16 Bytes, we can limit this also to 16 bits
    signal numlocators, numlocators_next : unsigned(SUBMESSAGE_LENGTH_WIDTH-1 downto 0) := (others => '0');
    -- Denotes if a suitable and valid Locator has been found
    signal locator_match, locator_match_next : std_logic := '0';
    -- Denotes if the Source of the Message is a Reader Endpoint
    signal src_is_reader, src_is_reader_next : std_logic := '0';
    -- Intermediate Write Enable Signal
    signal wr_sig : std_logic := '0';
    -- Submessage ID latch (Used as OPCODE by Endpoints)
    signal opcode, opcode_next : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := (others => '0');
    -- General Purpose counter
    signal cnt, cnt_next : natural range 0 to 10 := 0;
    -- Input in Big Endian representation
    signal data_in_swapped : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    -- Sequence Number latch
    signal sn_latch_1, sn_latch_1_next : SEQUENCENUMBER_TYPE := (others => (others => '0'));
    -- Sequence Number latch
    signal sn_latch_2, sn_latch_2_next : SEQUENCENUMBER_TYPE := (others => (others => '0'));
    -- Sequence Number latch
    signal sn_latch_3, sn_latch_3_next : SEQUENCENUMBER_TYPE := (others => (others => '0'));
    -- Long atch
    signal long_latch, long_latch_next : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0');
    -- Unsigned long latch
    signal ulong_latch, ulong_latch_next : unsigned(CDR_LONG_WIDTH-1 downto 0) := (others => '0');
    -- NumberSet Bitmap latch
    signal bitmap_latch, bitmap_latch_next : BITMAP_TYPE := (others => (others => '0'));
    -- Counter used to read out Bitmaps
    signal cnt2, cnt2_next : natural range 0 to BITMAP_TYPE'length := 0;
    -- Denotes the number of valid Bitmap longs (4-Byte words)
    signal bitmap_cnt, bitmap_cnt_next : unsigned(log2c(MAX_BITMAP_WIDTH/CDR_LONG_WIDTH)-1 downto 0) := (others => '0');
    -- Source Timestamp latch
    signal src_ts, src_ts_next : TIME_TYPE := TIME_INVALID;
    -- Indicates the Destination IP Address
    signal ip_addr_type, ip_addr_type_next : IPv4_ADDRESS_TYPE := INVALID;
    -- Alias "substitution"
    signal rtps_sub_length, rtps_sub_data_length : unsigned(SUBMESSAGE_LENGTH_WIDTH-1 downto 0) := (others => '0');
    
    --*****ALIAS DEFINATION*****
    -- UDP HEADER
    alias udp_src_port  : std_logic_vector(15 downto 0) is long_latch(31 downto 16);
    alias udp_dest_port : std_logic_vector(15 downto 0) is long_latch(15 downto 0);
    -- RTPS HEADER
    alias rtps_version  : std_logic_vector(15 downto 0) is data_in(31 downto 16);
    alias rtps_vendorid : std_logic_vector(15 downto 0) is data_in(15 downto 0);
    -- RTPS SUBMESSAGE HEADER
    alias rtps_sub_id           : std_logic_vector(7 downto 0) is data_in(31 downto 24);
    alias rtps_sub_flags        : std_logic_vector(7 downto 0) is data_in(23 downto 16);
    -- Apparently illegal alias expression
    --alias rtps_sub_length       : unsigned(15 downto 0) is unsigned(endian_swap(rtps_sub_flags(0), data_in(15 downto 0)));
    -- RTPS DATA SUBMESSAGE HEADER
    -- Apparently illegal alias expression
    --alias rtps_sub_data_length  : unsigned(15 downto 0) is unsigned(endian_swap(flags(SUBMESSAGE_ENDIAN_FLAG_POS), data_in(15 downto 0)));
    
    --*****FUNCTION DECLARATION*****
    
begin
    
    -- ALIAS SUBSTITUTION
    rtps_sub_length         <= unsigned(endian_swap(rtps_sub_flags(SUBMESSAGE_ENDIAN_FLAG_POS), data_in(15 downto 0)));
    rtps_sub_data_length    <= unsigned(endian_swap(flags(SUBMESSAGE_ENDIAN_FLAG_POS), data_in(15 downto 0)));
    
    rd  <= rd_sig;
    
    -- Big Endian Representation
    data_in_swapped <= endian_swap(flags(SUBMESSAGE_ENDIAN_FLAG_POS), data_in);
    
    -- This process connects the Intermediate Output Signals to the actual output FIFOs
    output_prc : process(all)
    begin
        --Write Enable Signal
        builtin_wr  <= '0';
        user_wr     <= (others => '0');
        if (wr_sig = '1') then
            if (builtin_endpoint = '1') then
                builtin_wr  <= '1';
            else
                user_wr     <= user_endpoint;
            end if;
        end if;
    end process;
    
    -- This process is responsible for reading the input FIFO 4-Byte Word aligned.
    -- Even though DDSI-RTPS 2.3 defines that Submessages begin at 4-byte boundries, meaning that the 
    -- submessage length is always a multiple of 4, the same is not defined for the "octetstoinlineQoS". 
    -- Therefore alignment is not guaranteed from the "SKIP_DATA_HEADER" stage on and has to be ensured explicitly.
    -- We store the lower 2 bits of the octet length (Which denotes the 4-Byte alignment offset), and together
    -- with the "align_sig" which stores 3 Bytes from the previous input word, we generate our aligned signal.
    align_prc : process(all)
        variable input : std_logic_vector(55 downto 0) := (others => '0');
    begin
        input := align_sig & data_in;
        case(align_offset) is
            when "01" =>
                data_in_aligned <= input(55 downto 24);
            when "10" =>
                data_in_aligned <= input(47 downto 16);
            when "11" =>
                data_in_aligned <= input(39 downto 8);
            when others => -- "00"
                data_in_aligned <= input(31 downto 0);
        end case;
    end process;
    
    -- Main State Machine
    -- STATE                    DESCRIPTION
    -- SRC_ADDR_HEADER          Initial and Idle state. Read IPv4 Source Address
    -- DEST_ADDR_HEADER         Read IPv4 Destination Address
    -- UDP_PORT_HEADER          Read UDP Source and Destination Ports
    -- LEN_HEADER               Read Packet Length (And check UDP Ports of previous stage)
    -- RTPS_HEADER_1            Parse first word of RTPS Header 
    -- RTPS_HEADER_2            Parse second word of RTPS Header 
    -- RTPS_HEADER_3            Parse GUID Prefix of RTPS Header (3rd-5th Word)
    -- RTPS_SUB_HEADER          Parse RTPS Submessage Header
    -- PARSE_INFO_DST           Parse InfoDestination Submessage
    -- PARSE_INFO_SRC           Parse InfoSource Submessage
    -- PARSE_INFO_TS            Parse InfoTimestamp Submessage
    -- PARSE_INFO_REPLY         Parse InfoReply Submessage
    -- EXTRACT_LOCATOR          Parse Locator (of InfoReply Submessage)
    -- PARSE_INFO_REPLY_IP4     Parse InfoReplyIP4 Submessage
    -- PARSE_HEARTBEAT          Parse Heartbeat Submessage
    -- PARSE_ACKNACK            Parse AckNack Submessage
    -- PARSE_GAP                Parse GAP Submessage
    -- PARSE_DATA               Parse DATA Submessage
    -- SKIP_DATA_HEADER         Skip uknown DATA Submessage Sub-Header part
    -- MATCH_DST_ENDPOINT       Determine destination of Submessage
    -- PUSH_PAYLOAD_HEADER      Write Payload Header into relevant output FIFOs
    -- PUSH_PAYLOAD             Write Payload into relevant output FIFOs (May read directly from input FIFO)
    -- SKIP_SUB                 Skip rest of Submessage
    -- SKIP_PACKET              Skip rest of UDP Packet
    parse_prc: process(all)
        variable tmp        : std_logic_vector(0 to NUM_ENDPOINTS-1) := (others => '0');
        variable dest       : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0');
        variable tmp_sn     : SEQUENCENUMBER_TYPE := SEQUENCENUMBER_UNKNOWN;
        -- This variable is used to allow the Overread Guard to control the rd Signal
        variable rd_guard   : std_logic := '0';
    begin
        --DEFAULT Registered
        stage_next                      <= stage;
        cnt_next                        <= cnt;
        align_offset_next               <= align_offset;
        align_sig_next                  <= align_sig;
        packet_length_next              <= packet_length;
        sub_end_next                    <= sub_end;
        offset_latch_next               <= offset_latch;
        src_addr_next                   <= src_addr;
        src_port_next                   <= src_port;
        flags_next                      <= flags;
        src_entityid_next               <= src_entityid;
        src_guidprefix_next             <= src_guidprefix;
        dest_entityid_next              <= dest_entityid;
        user_endpoint_next              <= user_endpoint;
        builtin_endpoint_next           <= builtin_endpoint;
        numlocators_next                <= numlocators;
        opcode_next                     <= opcode;
        data_header_end_next            <= data_header_end;
        locator_match_next              <= locator_match;
        is_metatraffic_next             <= is_metatraffic;
        src_is_reader_next              <= src_is_reader;
        sn_latch_1_next                 <= sn_latch_1;
        sn_latch_2_next                 <= sn_latch_2;
        sn_latch_3_next                 <= sn_latch_3;
        long_latch_next                 <= long_latch;
        ulong_latch_next                <= ulong_latch;
        bitmap_latch_next               <= bitmap_latch;
        cnt2_next                       <= cnt2;
        bitmap_cnt_next                 <= bitmap_cnt;
        src_ts_next                     <= src_ts;
        ip_addr_type_next               <= ip_addr_type;
        -- DEFAULT Unregistered
        data_out                        <= (others => '0');
        rd_sig                          <= '0';
        rd_guard                        := '0';
        reset_read_cnt                  <= '0';
        wr_sig                          <= '0';
        last_word_out                   <= '0';
        
        case(stage) is
            -- Initial/Idle State
            -- Source Address
            when SRC_ADDR_HEADER =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard        := '1';
                    reset_read_cnt  <= '1';
                    
                    src_addr_next       <= data_in;
                    
                    stage_next          <= DEST_ADDR_HEADER;
                end if;
            -- Destination Address
            when DEST_ADDR_HEADER =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard        := '1';
                    reset_read_cnt  <= '1';
                    
                    case (data_in) is
                        when DEFAULT_IPv4_META_ADDRESS =>
                            ip_addr_type_next   <= DEFAULT_META_ADDR;
                        when DEFAULT_IPv4_ADDRESS =>
                            ip_addr_type_next   <= DEFAULT_ADDR;
                        when others =>
                            ip_addr_type_next   <= INVALID;
                    end case;
                    
                    stage_next  <= UDP_PORT_HEADER;
                end if;
            -- UDP Src & Dest Ports
            when UDP_PORT_HEADER =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    
                    -- NOTE: Ports are latched for later checking, because we cannot yet skip the packet (Need to latch the packet length first)
                    long_latch_next     <= data_in;
                    
                    stage_next          <= LEN_HEADER;
                end if;
            -- Packet Length
            when LEN_HEADER =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard        := '1';
                    -- NOTE: Read word counter starts counting the moment we leave this stage. (We can skip Packets from this stage on)
                    reset_read_cnt  <= '1';
                    
                    packet_length_next  <= unsigned(data_in(packet_length'length-1 downto 0));
                    src_port_next       <= udp_src_port;
                    
                    -- DEFAULT
                    is_metatraffic_next <= '0';
                    stage_next          <= RTPS_HEADER_1;
                    
                    -- Check Address and Port Destination for validity
                    case (ip_addr_type) is
                        -- Only accept Metatraffic from the Default Multicast Address
                        when DEFAULT_META_ADDR =>
                            is_metatraffic_next <= '1';
                            case (udp_dest_port) is
                                when META_IPv4_MULTICAST_PORT =>
                                    null;
                                when META_IPv4_UNICAST_PORT =>
                                    null;
                                when others =>
                                    -- Ignore
                                    stage_next  <= SKIP_PACKET;
                            end case;
                        when DEFAULT_ADDR =>
                            case (udp_dest_port) is
                                when META_IPv4_MULTICAST_PORT =>
                                    is_metatraffic_next <= '1';
                                when META_IPv4_UNICAST_PORT =>
                                    is_metatraffic_next <= '1';
                                when USER_IPv4_MULTICAST_PORT =>
                                    null;
                                when USER_IPv4_UNICAST_PORT =>
                                    null;
                                when others =>
                                    -- Ignore
                                    stage_next  <= SKIP_PACKET;
                            end case;
                        -- Packet not destined for us
                        when others =>
                            -- Ignore
                            stage_next  <= SKIP_PACKET;
                    end case;
                end if;
            -- First RTPS Header word (Fields: Protocolld)
            when RTPS_HEADER_1 =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    
                    -- If underlying Protocol is not RTPS, skip packet
                    if(data_in /= PROTOCOL_RTPS) then
                        stage_next  <= SKIP_PACKET;
                    else
                        stage_next  <= RTPS_HEADER_2;
                    end if;
                end if;
            -- Second RTPS Header word (Fields: Protocol Version, Vendor ID)
            when RTPS_HEADER_2 =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    
                    -- If RTPS Protocol Major Version is not 2, skip packet
                    if(rtps_version(15 downto 8) /= PROTOCOLVERSION_2_4(15 downto 8)) then
                        stage_next  <= SKIP_PACKET;
                    else
                        stage_next  <= RTPS_HEADER_3;
                        cnt_next    <= 0;
                    end if;
                end if;
            -- Rest of RTPS Header (Fields: GUID Prefix)
            when RTPS_HEADER_3 =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        when 0 =>
                            src_guidprefix_next(0)  <= data_in;
                        when 1 =>
                            src_guidprefix_next(1)  <= data_in;
                        when 2 =>
                            src_guidprefix_next(2)  <= data_in;
                            
                            -- Reset
                            src_ts_next <= TIME_INVALID;
                            
                            stage_next  <= RTPS_SUB_HEADER;
                        when others =>
                            null;
                    end case;
                end if;
            -- RTPS Submessage Header (Fields: Submessage ID, Flags, Submessage Length)
            when RTPS_SUB_HEADER =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';

                    -- DEFAULT
                    src_is_reader_next  <= '0';
                    
                    flags_next          <= rtps_sub_flags;
                    opcode_next         <= rtps_sub_id;
                    
                    -- NOTE: DDSI-RTPS 2.3 only defines the 'INFO_TS' and 'PAD' Submessages to be able to have a zero Submessage Length.
                    --       If an unknown Submessage Type is encountered that can  also have a valid zero Submessage length (Without being the last Submessage), 
                    --       this will be seen here as "Last Submessage", and the complete rest of the Packet will be Skipped.
                    -- If Last Submessage, length is zero and actual size extend until end of packet
                    -- EXCEPTION: INFO_TS and PAD can have valid zero Submessage Length
                    if (rtps_sub_length = 0 and rtps_sub_id /= SID_PAD and rtps_sub_id /= SID_INFO_TS) then
                        -- Fix Submessage End Position
                        sub_end_next    <= packet_length;
                    else
                        -- NOTE: Submessage Length is always a multiple of four
                        sub_end_next    <= read_cnt_plus + rtps_sub_length(rtps_sub_length'length-1 downto 2);
                    end if;
                    
                    case (rtps_sub_id) is
                        -- INFO_DST (Writer -> Reader, Update Destination GUID Prefix)
                        when SID_INFO_DST =>
                            stage_next  <= PARSE_INFO_DST;
                            cnt_next    <= 0;
                        -- INFO_SRC (RTPS Header in Submessage form)
                        when SID_INFO_SRC =>
                            stage_next  <= PARSE_INFO_SRC;
                            cnt_next    <= 0;
                        -- INFO_TS (Source Timestamp)
                        when SID_INFO_TS =>
                            -- Invalidate Stored Timestamp
                            if (rtps_sub_flags(SUBMESSAGE_INVALIDATE_FLAG_POS) = '1') then
                                src_ts_next <= TIME_INVALID;
                                stage_next  <= SKIP_SUB;
                            else
                                stage_next  <= PARSE_INFO_TS;
                                cnt_next    <= 0;
                            end if;
                        -- INFO_REPLY (Source Port and Address)
                        when SID_INFO_REPLY =>
                            stage_next  <= PARSE_INFO_REPLY;
                        -- INFO_REPLY (Source Port and Address)
                        when SID_INFO_REPLY_IP4 =>
                            stage_next  <= PARSE_INFO_REPLY_IP4;
                            cnt_next    <= 0;
                        -- Heartbeat (Writer -> Reader, Available SeqNum)
                        when SID_HEARTBEAT =>
                            stage_next  <= PARSE_HEARTBEAT;
                            cnt_next    <= 0;
                        -- AckNack (Reader -> Writer, Request SeqNum)
                        when SID_ACKNACK =>
                            -- Even though this Submessage is Ignored, it is parsed to check Validity
                            stage_next  <= PARSE_ACKNACK;
                            cnt_next    <= 0;
                        -- GAP (Writer -> Reader, Invalidate SeqNum)
                        when SID_GAP =>
                            stage_next  <= PARSE_GAP;
                            cnt_next    <= 0;
                        -- DATA (Writer -> Reader, SeqNum+Data)
                        when SID_DATA =>                            
                            stage_next  <= PARSE_DATA;
                            cnt_next    <= 0;
                            
                            -- SANITY CHECK
                            if (rtps_sub_flags(SUBMESSAGE_INLINE_QOS_FLAG_POS) = '0' and rtps_sub_flags(SUBMESSAGE_DATA_FLAG_POS) = '0' and rtps_sub_flags(SUBMESSAGE_KEY_FLAG_POS) = '0') then
                                -- Submessage has no Data, skip
                                stage_next  <= SKIP_SUB;
                            -- VALIDITY CHECK
                            elsif (rtps_sub_flags(SUBMESSAGE_DATA_FLAG_POS) = '1' and rtps_sub_flags(SUBMESSAGE_KEY_FLAG_POS) = '1') then
                                -- Invalid Submessage, skip Packet (see DDSI-RTPS 2.3 Section 9.4.5.3.1 and 8.3.4.1)
                                -- TODO: Clarify if this invalidate the rest of the Message, since it is not stated in 8.3.7.2.3
                                stage_next  <= SKIP_SUB;
                            end if;
                        -- PAD (Variable Size Padding)
                        when SID_PAD =>
                            stage_next  <= SKIP_SUB;
                        -- Unknown ID, skip submessage
                        when others =>
                            stage_next  <= SKIP_SUB;
                    end case;
                end if;
            when PARSE_INFO_DST =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    -- If Destination GUID Prefix is not us, skip the rest of the packet
                    case (cnt) is
                        -- GUID Prefix 1/3
                        when 0 =>
                            if (data_in /= GUIDPREFIX(0)) then
                                -- Ignore
                                stage_next  <= SKIP_PACKET;
                            end if;
                        -- GUID Prefix 2/3
                        when 1 =>
                            if (data_in /= GUIDPREFIX(1)) then
                                -- Ignore
                                stage_next  <= SKIP_PACKET;
                            end if;
                        -- GUID Prefix 3/3
                        when 2 =>
                            if (data_in /= GUIDPREFIX(2)) then
                                -- Ignore
                                stage_next  <= SKIP_PACKET;
                            else
                                stage_next  <= SKIP_SUB;
                            end if;
                        when others =>
                            null;
                    end case;
                end if;
            when PARSE_INFO_SRC =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- unused
                        when 0 =>
                            null;
                        -- Protocol Version & Vendor ID
                        when 1 =>
                            -- Check Major Protocol Version
                            if (data_in(31 downto 24) /= PROTOCOLVERSION_2_4(15 downto 8)) then
                                -- Protocol not supported, skip rest of Packet
                                stage_next  <= SKIP_PACKET;
                            end if;
                        -- GUID Prefix 1/3
                        when 2 =>
                            src_guidprefix_next(0)  <= data_in;
                        -- GUID Prefix 2/3
                        when 3 =>
                            src_guidprefix_next(1)  <= data_in;
                        -- GUID Prefix 3/3
                        when 4 =>
                            src_guidprefix_next(2)  <= data_in;
                            stage_next              <= SKIP_SUB;
                        when others =>
                            null;
                    end case;
                end if;
            when PARSE_INFO_TS =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- Timesatmp 1/2
                        when 0 =>
                            src_ts_next(0)  <= unsigned(data_in_swapped);
                        -- Timesatmp 2/2
                        when 1 =>
                            src_ts_next(1)  <= unsigned(data_in_swapped);
                            
                            -- DONE
                            stage_next  <= SKIP_SUB;
                        when others =>
                            null;
                    end case;
                end if;
            when PARSE_INFO_REPLY =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    
                    -- Extract Locator List
                    numlocators_next    <= unsigned(data_in_swapped(numlocators_next'length-1 downto 0));
                    
                    stage_next          <= EXTRACT_LOCATOR;
                    cnt_next            <= 0;
                end if;
            when EXTRACT_LOCATOR =>
                -- Check Number of Locators
                if (numlocators = 0) then
                    -- Check if Multicast Locator List exists
                    if (flags(SUBMESSAGE_MULTICAST_FLAG_POS) = '1') then
                        -- Reset Flag to prevent loop
                        flags_next(SUBMESSAGE_MULTICAST_FLAG_POS) <= '0';
                        
                        stage_next  <= PARSE_INFO_REPLY;
                    -- No further Locators, next Submessage
                    else
                        stage_next  <= SKIP_SUB;
                    end if;
                else
                    -- Input FIFO Guard
                    if (empty = '0') then
                        rd_guard    := '1';
                        cnt_next    <= cnt + 1;
                        
                        case (cnt) is
                            -- Locator Kind
                            when 0 =>
                                -- Check if UDPv4 Locator
                                if (data_in_swapped = LOCATOR_KIND_UDPv4) then
                                    locator_match_next  <= '1';
                                else
                                    locator_match_next  <= '0';
                                end if;
                            -- Locator Port
                            when 1 =>
                                -- We only store UDPv4 Locators
                                if (locator_match = '1') then
                                    -- We only store valid Locators
                                    if (data_in_swapped(UDP_PORT_INVALID'range) = UDP_PORT_INVALID) then
                                        locator_match_next  <= '0';
                                    else
                                        long_latch_next     <= data_in_swapped;
                                    end if;
                                end if;
                            -- Locator Address 1/4
                            when 2 =>
                                null;
                            -- Locator Address 2/4
                            when 3 =>
                                null;
                            -- Locator Address 3/4
                            when 4 =>
                                null;
                            -- Locator Address 4/4 (IPv4 Address)
                            when 5 =>
                                -- We only store valid UDPv4 Locators
                                if (locator_match = '1' and data_in /= IPv4_ADDRESS_INVALID) then
                                    src_addr_next   <= data_in;
                                    src_port_next   <= long_latch(src_port_next'length-1 downto 0);
                                end if;
                                -- Last Word of Locator
                                numlocators_next    <= numlocators - 1;
                                cnt_next            <= 0;
                            when others =>
                                null;
                        end case;
                    end if;
                end if;
            when PARSE_INFO_REPLY_IP4 =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- IPv4 Address
                        when 0 =>
                            -- Store only valid Locators
                            if (data_in_swapped = IPv4_ADDRESS_INVALID) then
                                locator_match_next  <= '0';
                            else
                                locator_match_next  <= '1';
                                long_latch_next     <= data_in_swapped;
                            end if;
                        -- UDPv4 Port
                        when 1 =>
                            -- Store only valid Locators
                            if (locator_match = '1' and data_in_swapped(UDP_PORT_INVALID'range) /= UDP_PORT_INVALID) then
                                src_port_next   <= data_in_swapped(src_port_next'length-1 downto 0);
                                src_addr_next   <= long_latch;
                            end if;
                            
                            -- Parse Multicast if available
                            if (flags(SUBMESSAGE_MULTICAST_FLAG_POS) = '1') then
                                -- Reset Flag to prevent loop
                                flags_next(SUBMESSAGE_MULTICAST_FLAG_POS) <= '0';
                                
                                cnt_next            <= 0;
                            else
                                -- DONE
                                stage_next  <= SKIP_SUB;
                            end if;
                        when others =>
                            null;
                    end case;
                end if;
            when PARSE_HEARTBEAT =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- Reader Entity ID
                        when 0 =>
                            dest_entityid_next  <= data_in;
                        -- Writer Entity ID
                        when 1 =>
                            src_entityid_next   <= data_in;
                        -- First Sequence Number 1/2
                        when 2 =>
                            sn_latch_1_next(0)  <= unsigned(data_in_swapped);
                        -- First Sequence Number 2/2
                        when 3 =>
                            sn_latch_1_next(1)  <= unsigned(data_in_swapped);
                            
                            -- VALIDITY CHECK
                            tmp_sn  := (0 => sn_latch_1(0), 1 => unsigned(data_in_swapped));
                            if (tmp_sn = 0 or tmp_sn(0)(WORD_WIDTH-1) = '1') then
                                -- If firstSN.value is zero or negative, skip Packet (see DDSI-RTPS 2.3 Section 8.3.7.5.3 and 8.3.4.1)
                                stage_next  <= SKIP_PACKET;
                            end if;
                        -- Last Sequence Number 1/2
                        when 4 =>
                            sn_latch_2_next(0)  <= unsigned(data_in_swapped);
                            
                            -- Pre-Calculation for Validity Check
                            sn_latch_3_next     <= sn_latch_1 - 1;
                        -- Last Sequence Number 2/2
                        when 5 =>
                            sn_latch_2_next(1)  <= unsigned(data_in_swapped);
                            
                            -- VALIDITY CHECK
                            tmp_sn  := (0 => sn_latch_2(0), 1 => unsigned(data_in_swapped));
                            if (tmp_sn(0)(WORD_WIDTH-1) = '1') then
                                -- If lastSN.value is negative, skip Packet (see DDSI-RTPS 2.3 Section 8.3.7.5.3 and 8.3.4.1)
                                stage_next  <= SKIP_PACKET;
                            end if;
                        -- Count
                        when 6 =>
                            long_latch_next    <= data_in_swapped;
                            
                            if (sn_latch_2 < sn_latch_3) then
                                -- If lastSN.value < firstSN.value - 1, skip Packet (see DDSI-RTPS 2.3 Section 8.3.7.5.3 and 8.3.4.1)
                                stage_next  <= SKIP_PACKET;
                            else
                                -- DONE
                                stage_next  <= MATCH_DST_ENDPOINT;
                            end if;
                        when others =>
                            null;
                    end case;
                end if;
            when PARSE_ACKNACK =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- Reader Entity ID
                        when 0 =>
                            src_is_reader_next  <= '1';
                            src_entityid_next   <= data_in;
                        -- Writer Entity ID
                        when 1 =>
                            dest_entityid_next  <= data_in;
                        -- ReaderSNState.Base 1/2
                        when 2 =>
                            sn_latch_1_next(0)  <= unsigned(data_in_swapped);
                        -- ReaderSNState.Base 2/2
                        when 3 =>
                            sn_latch_1_next(1)  <= unsigned(data_in_swapped);
                            
                            -- VALIDITY CHECK
                            tmp_sn  := (0 => sn_latch_1(0), 1 => unsigned(data_in_swapped));
                            if (tmp_sn = 0 or tmp_sn(0)(WORD_WIDTH-1) = '1') then
                                -- If Bitmap Base is zero or negative, Number Set is invalid (see DDSI-RTPS 2.3 Section 9.4.2.6)
                                -- If readerSNState is invalid, skip Packet (see DDSI-RTPS 2.3 Section 8.3.7.1.3 and 8.3.4.1)
                                stage_next  <= SKIP_PACKET;
                            end if;
                        -- ReaderSNState.NumBits
                        when 4 =>
                            ulong_latch_next    <= unsigned(data_in_swapped);
                            bitmap_cnt_next     <= unsigned(round_slv(data_in_swapped(log2c(MAX_BITMAP_WIDTH)-1 downto 0),bitmap_cnt'length));
                            cnt2_next           <= 0;
                            
                            -- VALIDITY CHECK
                            if (data_in_swapped(0) = '1' or unsigned(data_in_swapped) > 256) then
                                -- If numBits is negative or larger than 256, Number Set is invalid (see DDSI-RTPS 2.3 Section 9.4.2.6)
                                -- If readerSNState is invalid, skip Packet (see DDSI-RTPS 2.3 Section 8.3.7.1.3 and 8.3.4.1)
                                stage_next  <= SKIP_PACKET;
                            end if;
                        -- ReaderSNState.Bitmap
                        when 5 =>
                            -- Read Bitmap
                            if (cnt2 < bitmap_cnt) then
                                cnt2_next <= cnt2 + 1;
                                
                                bitmap_latch_next(cnt2) <= data_in_swapped;
                                
                                -- Keep Sub-State
                                cnt_next    <= cnt;
                            -- Exit Condition
                            else
                                -- Prevent Input Latching
                                rd_guard    := '0';
                            end if;
                        -- Count
                        when 6 =>
                            long_latch_next <= data_in_swapped;
                            
                            -- DONE
                            stage_next      <= MATCH_DST_ENDPOINT;
                        when others =>
                            null;
                    end case;
                end if;
            when PARSE_GAP =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- Reader Entity ID
                        when 0 =>
                            dest_entityid_next  <= data_in;
                        -- Writer Entity ID
                        when 1 =>
                            src_entityid_next   <= data_in;
                        -- GapStart Sequence Number  1/2
                        when 2 =>
                            sn_latch_1_next(0)  <= unsigned(data_in_swapped);
                        -- GapStart Sequence Number  2/2
                        when 3 =>
                            sn_latch_1_next(1)  <= unsigned(data_in_swapped);
                            
                            -- VALIDITY CHECK
                            tmp_sn  := (0 => sn_latch_1(0), 1 => unsigned(data_in_swapped));
                            if (tmp_sn = 0 or tmp_sn(0)(WORD_WIDTH-1) = '1') then
                                -- If Bitmap Base is zero or negative, Number Set is invalid (see DDSI-RTPS 2.3 Section 9.4.2.6)
                                -- If gapList is invalid, skip Packet (see DDSI-RTPS 2.3 Section 8.3.7.4.3 and 8.3.4.1)
                                stage_next  <= SKIP_PACKET;
                            end if;
                        -- GapList.Base 1/2
                        when 4 =>
                            sn_latch_2_next(0)  <= unsigned(data_in_swapped);
                        -- GapList.Base 2/2
                        when 5 =>
                            sn_latch_2_next(1)  <= unsigned(data_in_swapped);
                            
                            -- VALIDITY CHECK
                            tmp_sn  := (0 => sn_latch_2(0), 1 => unsigned(data_in_swapped));
                            if (tmp_sn = 0 or tmp_sn(0)(WORD_WIDTH-1) = '1') then
                                -- If Bitmap Base is zero or negative, Number Set is invalid (see DDSI-RTPS 2.3 Section 9.4.2.6)
                                -- If gapList is invalid, skip Packet (see DDSI-RTPS 2.3 Section 8.3.7.4.3 and 8.3.4.1)
                                stage_next  <= SKIP_PACKET;
                            end if;
                        -- ReaderSNState.NumBits
                        when 6 =>
                            ulong_latch_next    <= unsigned(data_in_swapped);
                            bitmap_cnt_next     <= unsigned(round_slv(data_in_swapped(log2c(MAX_BITMAP_WIDTH)-1 downto 0),bitmap_cnt'length));
                            cnt2_next           <= 0;
                            
                            -- VALIDITY CHECK
                            -- TODO: Shouldn't that check the highest bit?
                            if (data_in_swapped(0) = '1' or unsigned(data_in_swapped) > 256) then
                                -- If numBits is negative or larger than 256, Number Set is invalid (see DDSI-RTPS 2.3 Section 9.4.2.6)
                                -- If gapList is invalid, skip Packet (see DDSI-RTPS 2.3 Section 8.3.7.4.3 and 8.3.4.1)
                                stage_next  <= SKIP_PACKET;
                            end if;
                        -- ReaderSNState.Bitmap
                        when 7 =>
                            -- Read Bitmap
                            if (cnt2 < bitmap_cnt) then
                                cnt2_next <= cnt2 + 1;
                                
                                bitmap_latch_next(cnt2) <= data_in_swapped;
                                
                                -- Keep Sub-State
                                cnt_next    <= cnt;
                            end if;
                        when others =>
                            null;
                    end case;
                end if;
                -- Exit Condition (Not influenced by Empty Signal)
                if (cnt = 7 and cnt2 >= bitmap_cnt) then
                    -- Prevent Input Latching
                    rd_guard    := '0';
                    -- DONE
                    stage_next  <= MATCH_DST_ENDPOINT;
                end if;
            when PARSE_DATA =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- Extra Flags & octetstoinlineQoS
                        when 0 =>
                            -- NOTE: Extra Flags are unused
                            -- Latch Length to skip Uknown Data Header Part and latch offset to ensure 4-Byte alignement (see align_prc)
                            offset_latch_next       <= std_logic_vector(rtps_sub_data_length(1 downto 0));
                            data_header_end_next    <= (read_cnt_plus & "00") + rtps_sub_data_length;
                        -- Reader Entity ID
                        when 1 =>
                            dest_entityid_next  <= data_in;
                        -- Writer Entity ID
                        when 2 =>
                            src_entityid_next   <= data_in;
                        -- Sequence Number  1/2
                        when 3 =>
                            sn_latch_1_next(0)  <= unsigned(data_in_swapped);
                        -- Sequence Number  2/2
                        when 4 =>
                            sn_latch_1_next(1)  <= unsigned(data_in_swapped);
                            
                            -- VALIDITY CHECK
                            tmp_sn  := (0 => sn_latch_1(0), 1 => unsigned(data_in_swapped));
                            if (tmp_sn = 0 or tmp_sn(0)(WORD_WIDTH-1) = '1') then
                                -- If Sequence Number is zero or negative, skip Packet (see DDSI-RTPS 2.3 Section 8.3.7.2.3 and 8.3.4.1)
                                -- NOTE: The actual wording used is "not strictly positive (1, 2, ...) or is SEQUENCENUMBER_UNKNOWN"
                                stage_next  <= SKIP_PACKET;
                            else
                                stage_next  <= SKIP_DATA_HEADER;
                            end if;
                        when others =>
                            null;
                    end case;
                end if;
            when SKIP_DATA_HEADER =>
                -- End of Data Header
                if ((read_cnt & "00") >= data_header_end) then
                    stage_next          <= MATCH_DST_ENDPOINT;
                    cnt_next            <= 0;
                    -- Fix alignement
                    align_offset_next   <= offset_latch;
                -- Input Guard
                elsif(empty = '0') then
                    -- Latch Input for alignment purposes
                    align_sig_next      <= data_in(23 downto 0);
                    -- Skip-Read
                    rd_guard    := '1';
                end if;
            when MATCH_DST_ENDPOINT =>
                -- DEFAULT 
                user_endpoint_next      <= (others => '0');
                builtin_endpoint_next   <= '0';
                stage_next              <= PUSH_PAYLOAD_HEADER;
                cnt_next                <= 0;
                
                -- *Check Dest Entity ID*
                -- Target ALL Endpoints
                if (dest_entityid = ENTITYID_UNKNOWN) then
                    if (is_metatraffic = '1') then
                        builtin_endpoint_next   <= '1';
                    else
                        -- Mark Only Writers
                        if (src_is_reader = '1' and NUM_WRITERS /= 0) then
                            user_endpoint_next  <= not ENDPOINT_READERS;
                        -- Mark Only Readers
                        elsif (NUM_READERS /= 0) then
                            user_endpoint_next  <= ENDPOINT_READERS;
                        end if;
                    end if;
                -- Target Built-In Endpoints
                elsif (is_metatraffic = '1' and dest_entityid(7 downto 6) = BUILT_IN_ENTITY) then
                    builtin_endpoint_next   <= '1';
                -- Match User Entity ID
                elsif (is_metatraffic = '0') then
                    
                    tmp := (others => '0');
                    
                    for i in 0 to ENTITYID'length-1 loop
                        if (dest_entityid = ENTITYID(i)) then
                            tmp(i)  := '1';
                        end if;
                    end loop;
                    
                    -- Entity non existent, skip Submessage
                    if (tmp = (tmp'range => '0')) then
                        stage_next  <= SKIP_SUB;
                    -- Entity ID Match
                    else
                        -- SANITY CHECK: Allow only Reader-Writer Communication
                        if (src_is_reader = '1') then
                            -- Mark only Writers
                            user_endpoint_next  <= tmp and (not ENDPOINT_READERS);
                        else
                            -- Mark only Readers
                            user_endpoint_next  <= tmp and ENDPOINT_READERS;
                        end if;
                    end if;
                -- Destination Unreachable, skip Submessage
                else
                    stage_next  <= SKIP_SUB;
                end if;
            when PUSH_PAYLOAD_HEADER =>
                -- NOTE: This is a synchronised push on potentially multiple output FIFOs. If one FIFO gets full, the process stalls for all FIFOs.
                -- Output FIFO Guard
                if (builtin_endpoint = '1' and builtin_full = '0') or (builtin_endpoint = '0' and ((user_endpoint and user_full) = (user_endpoint'range => '0'))) then
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- OPCODE (Submessage ID), Submessage Flags, UPDv4 Source Port
                        when 0 =>
                            data_out    <= opcode & flags & src_port;
                            wr_sig      <= '1';
                        -- IPv4 Source Address
                        when 1 =>
                            data_out    <= src_addr;
                            wr_sig      <= '1';
                        -- GUID Prefix 1/3
                        when 2 =>
                            data_out    <= src_guidprefix(0);
                            wr_sig      <= '1';
                        -- GUID Prefix 2/3
                        when 3 =>
                            data_out    <= src_guidprefix(1);
                            wr_sig      <= '1';
                        -- GUID Prefix 3/3
                        when 4 =>
                            data_out    <= src_guidprefix(2);
                            wr_sig      <= '1';
                        -- Source Entity ID
                        when 5 => 
                            data_out    <= src_entityid;
                            wr_sig      <= '1';
                        -- Destination ID [only for Built-in Endpoints]
                        when 6 =>
                            if (builtin_endpoint = '1') then
                                data_out    <= dest_entityid;
                                wr_sig      <= '1';
                            end if;
                        -- Sequence Number 1/2 [only for DATA Submessages]
                        when 7 =>
                            if (opcode = SID_DATA) then
                                data_out    <= std_logic_vector(sn_latch_1(0));
                                wr_sig      <= '1';
                            end if;
                        -- Sequence Number 2/2 [only for DATA Submessages]
                        when 8 =>
                            if (opcode = SID_DATA) then
                                data_out    <= std_logic_vector(sn_latch_1(1));
                                wr_sig      <= '1';
                            end if;
                        -- Source Timestamp 1/2 [only for DATA Submessages and User Endpoints]
                        when 9 =>
                            if (opcode = SID_DATA and builtin_endpoint = '0') then
                                data_out    <= std_logic_vector(src_ts(0));
                                wr_sig      <= '1';
                            end if;
                        -- Source Timestamp 1/2 [only for DATA Submessages and User Endpoints]
                        when 10 =>
                            if (opcode = SID_DATA and builtin_endpoint = '0') then
                                data_out    <= std_logic_vector(src_ts(1));
                                wr_sig      <= '1';
                            end if;
                            
                            stage_next  <= PUSH_PAYLOAD;
                            cnt_next    <= 0;
                        when others =>
                            null;
                    end case;
                end if;
            when PUSH_PAYLOAD =>
                -- NOTE: This is a synchronised push on potentially multiple output FIFOs. If one FIFO gets full, the process stalls for all FIFOs.
                -- Output FIFO Guard
                if (builtin_endpoint = '1' and builtin_full = '0') or (builtin_endpoint = '0' and ((user_endpoint and user_full) = (user_endpoint'range => '0'))) then
                    cnt_next    <= cnt + 1;
                    
                    case (opcode) is
                        when SID_HEARTBEAT =>
                            case (cnt) is
                                -- FirstSN 1/2
                                when 0 =>
                                    data_out    <= std_logic_vector(sn_latch_1(0));
                                    wr_sig      <= '1';
                                -- FirstSN 2/2
                                when 1 =>
                                    data_out    <= std_logic_vector(sn_latch_1(1));
                                    wr_sig      <= '1';
                                -- LastSN 1/2
                                when 2 =>
                                    data_out    <= std_logic_vector(sn_latch_2(0));
                                    wr_sig      <= '1';
                                -- LastSN 2/2
                                when 3 =>
                                    data_out    <= std_logic_vector(sn_latch_2(1));
                                    wr_sig      <= '1';
                                -- Count
                                when 4 =>
                                    data_out        <= long_latch;
                                    wr_sig          <= '1';
                                    last_word_out   <= '1';
                                    
                                    -- DONE
                                    stage_next      <= SKIP_SUB;
                                when others =>
                                    null;
                            end case;
                        when SID_ACKNACK =>
                            case (cnt) is
                                -- ReaderSNState.Base 1/2
                                when 0 =>
                                    data_out    <= std_logic_vector(sn_latch_1(0));
                                    wr_sig      <= '1';
                                -- ReaderSNState.Base 2/2
                                when 1 =>
                                    data_out    <= std_logic_vector(sn_latch_1(1));
                                    wr_sig      <= '1';
                                -- ReaderSNState.NumBits
                                when 2 =>
                                    data_out    <= std_logic_vector(ulong_latch);
                                    wr_sig      <= '1';
                                    cnt2_next   <= 0;
                                -- ReaderSNState.Bitmap
                                when 3 =>
                                    -- Write Bitmap
                                    if (cnt2 < bitmap_cnt) then
                                        cnt2_next <= cnt2 + 1;
                                        
                                        data_out    <= bitmap_latch_next(cnt2);
                                        wr_sig      <= '1';
                                        
                                        -- Keep Sub-State
                                        cnt_next    <= cnt;
                                    end if;
                                -- Count
                                when 4 =>
                                    data_out        <= long_latch;
                                    wr_sig          <= '1';
                                    last_word_out   <= '1';
                                    
                                    -- DONE
                                    stage_next      <= SKIP_SUB;
                                when others =>
                                    null;
                            end case;
                        when SID_GAP =>
                            case (cnt) is
                                -- GapStart 1/2
                                when 0 =>
                                    data_out    <= std_logic_vector(sn_latch_1(0));
                                    wr_sig      <= '1';
                                -- GapStart 2/2
                                when 1 =>
                                    data_out    <= std_logic_vector(sn_latch_1(1));
                                    wr_sig      <= '1';
                                -- GapList.Base 1/2
                                when 2 =>
                                    data_out    <= std_logic_vector(sn_latch_2(0));
                                    wr_sig      <= '1';
                                -- GapList.Base 2/2
                                when 3 =>
                                    data_out    <= std_logic_vector(sn_latch_2(1));
                                    wr_sig      <= '1';
                                -- GapList.NumBits
                                when 4 =>
                                    data_out    <= std_logic_vector(ulong_latch);
                                    wr_sig      <= '1';
                                    cnt2_next   <= 0;
                                -- GapList.Bitmap
                                when 5 =>
                                    -- Write Bitmap
                                    if (cnt2 < bitmap_cnt) then
                                        cnt2_next <= cnt2 + 1;
                                        
                                        data_out    <= bitmap_latch_next(cnt2);
                                        wr_sig      <= '1';
                                        
                                        -- Keep Sub-State
                                        cnt_next    <= cnt;
                                    end if;
                                -- NOTE: Because we need to pull "last_word_out" high on the last Byte, and doing so in the last Byte of Bitmap was deemed
                                --       to much overhead (We need a counter in addition to cnt2 to track the next-to-last Byte of the Bitmap), we just 
                                --       define the Frame with an additional dummy word in the end.
                                -- UNUSED
                                when 6 =>
                                    data_out        <= (others => '0');
                                    wr_sig          <= '1';
                                    last_word_out   <= '1';
                                    
                                    -- DONE
                                    stage_next      <= SKIP_SUB;
                                when others =>
                                    null;
                            end case;
                        when SID_DATA =>
                            -- Last Payload Word
                            if (read_cnt = sub_end) then
                                -- Begin parsing of next submessage
                                stage_next          <= RTPS_SUB_HEADER;
                                -- Reset alignement
                                align_offset_next   <= (others => '0');
                                -- Reset Submessage End
                                sub_end_next        <= (others => '1');
                            -- Input FIFO Guard
                            elsif (empty = '0') then
                                rd_guard    := '1';
                        
                                -- Latch Input for alignment purposes
                                align_sig_next  <= data_in(23 downto 0);
                                
                                -- Push Payload
                                data_out        <= data_in_aligned;
                                wr_sig          <= '1';
                                
                                if (read_cnt_plus = sub_end) then
                                    last_word_out       <= '1';
                                    -- Begin parsing of next submessage
                                    stage_next          <= RTPS_SUB_HEADER;
                                    -- Reset alignement
                                    align_offset_next   <= (others => '0');
                                    -- Reset Submessage End
                                    sub_end_next        <= (others => '1');
                                end if;
                            end if;
                        when others =>
                            stage_next  <= SKIP_SUB;
                    end case;
                end if;
            when SKIP_SUB =>
                -- End of Packet
                if (read_cnt = packet_length) then
                    -- Continue parsing next Packet
                    stage_next          <= SRC_ADDR_HEADER;
                    -- Reset Lengths
                    packet_length_next  <= (others => '1');
                    sub_end_next        <= (others => '1');
                -- End of Submessage
                elsif (read_cnt = sub_end) then
                    -- Begin parsing of next submessage
                    stage_next      <= RTPS_SUB_HEADER;
                    -- Reset Submessage End
                    sub_end_next    <= (others => '1');
                -- Input FIFO Guard
                elsif (empty = '0') then
                    -- Skip-Read
                    rd_guard    := '1';
                end if;
            when SKIP_PACKET =>
                -- Reset Submessage End
                sub_end_next    <= (others => '1');
                
                -- End of Packet
                if (read_cnt = packet_length) then
                    -- Continue parsing next Packet
                    stage_next          <= SRC_ADDR_HEADER;
                    -- Reset Packet Length
                    packet_length_next  <= (others => '1');
                -- Input FIFO Guard
                elsif (empty = '0') then
                    -- Skip-Read
                    rd_guard    := '1';
                end if;
                -- NOTE: Exit condition is via the OVERREAD GUARD
            when others =>
                null;
        end case;
        
        -- OVERREAD GUARD
        -- Read outside of packet Length
        -- NOTE: If the Packet Length is smaller than expected there will be a read from input FIFO while
        --       the Packet Length has been reached and will be caught by this clause.
        --       The SKIP_PACKET clause prevents a read signal from occuring in this situation, and thus prevents from entering this state.
        if (read_cnt = packet_length and rd_guard = '1') then
            -- Force rd_sig low
            rd_sig              <= '0';
            -- Notify Endpoints of EOP
            last_word_out       <= '1';
            -- Continue parsing next Packet
            stage_next          <= SRC_ADDR_HEADER;
            -- Reset Lengths
            packet_length_next  <= (others => '1');
            sub_end_next        <= (others => '1');
        -- Read outside of Submessage Length
        -- NOTE: If the Submessage Length is smaller than expected for a particular Submessage, there will be a read from input FIFO while
        --       the Submessage Length has been reached and will be caught by this clause.
        --       The SKIP_SUB clause prevents a read signal from occuring in this situation, and thus prevents from entering this state.
        elsif (read_cnt = sub_end and rd_guard = '1') then
            -- Force rd_sig low
            rd_sig          <= '0';
            -- Notify Endpoints of EOS
            last_word_out   <= '1';
            -- Invalid Submessage Length Field, Skip Packet (see DDSI-RTPS 2.3 Section 8.3.4.1)
            stage_next      <= SKIP_PACKET;
            -- Reset Submessage End
            sub_end_next    <= (others => '1');
        -- DEFAULT
        else
            rd_sig          <= rd_guard;
        end if;
    end process;
    
    -- Process responsible for counting read words
    -- This process uses the actual FIFO read signals to determine reads
    word_counter_prc : process(clk, reset)
    begin
        if rising_edge(clk) then
            -- Reset Read counter
            if (reset = '1' or reset_read_cnt = '1') then
                read_cnt        <= (others => '0');
                read_cnt_plus   <= to_unsigned(1, read_cnt_plus'length);
            -- Increment read counter each time rd is high
            elsif (rd_sig = '1') then
                read_cnt        <= read_cnt + 1;
                read_cnt_plus   <= read_cnt_plus + 1;
            end if;
        end if;
    end process;
    
    sync: process(clk)
    begin
        if rising_edge(clk) then
            if (reset = '1') then
                stage                       <= SRC_ADDR_HEADER;
                ip_addr_type                <= INVALID;
                packet_length               <= (others => '1');
                sub_end                     <= (others => '1');
                align_offset                <= (others => '0');
                align_sig                   <= (others => '0');
                offset_latch                <= (others => '0');
                src_addr                    <= (others => '0');                
                src_port                    <= (others => '0');
                flags                       <= (others => '0');
                src_entityid                <= (others => '0');
                dest_entityid               <= (others => '0');
                user_endpoint               <= (others => '0');
                numlocators                 <= (others => '0');
                opcode                      <= (others => '0');
                data_header_end             <= (others => '0');
                long_latch                  <= (others => '0');
                ulong_latch                 <= (others => '0');
                bitmap_cnt                  <= (others => '0');
                bitmap_latch                <= (others => (others => '0'));
                src_ts                      <= TIME_INVALID;
                src_guidprefix              <= GUIDPREFIX_UNKNOWN;
                sn_latch_1                  <= SEQUENCENUMBER_UNKNOWN;
                sn_latch_2                  <= SEQUENCENUMBER_UNKNOWN;
                sn_latch_3                  <= SEQUENCENUMBER_UNKNOWN;
                cnt                         <= 0;
                cnt2                        <= 0;
                src_is_reader               <= '0';
                locator_match               <= '0';
                builtin_endpoint            <= '0';
                is_metatraffic              <= '0';
            else
                stage                       <= stage_next;
                ip_addr_type                <= ip_addr_type_next;
                align_offset                <= align_offset_next;
                align_sig                   <= align_sig_next;
                packet_length               <= packet_length_next;
                sub_end                     <= sub_end_next;
                offset_latch                <= offset_latch_next;
                src_addr                    <= src_addr_next;
                src_port                    <= src_port_next;
                flags                       <= flags_next;
                src_entityid                <= src_entityid_next;
                dest_entityid               <= dest_entityid_next;
                user_endpoint               <= user_endpoint_next;
                numlocators                 <= numlocators_next;
                opcode                      <= opcode_next;
                data_header_end             <= data_header_end_next;
                long_latch                  <= long_latch_next;
                ulong_latch                 <= ulong_latch_next;
                bitmap_cnt                  <= bitmap_cnt_next;
                bitmap_latch                <= bitmap_latch_next;
                src_ts                      <= src_ts_next;
                src_guidprefix              <= src_guidprefix_next;
                sn_latch_1                  <= sn_latch_1_next;
                sn_latch_2                  <= sn_latch_2_next;
                sn_latch_3                  <= sn_latch_3_next;
                cnt                         <= cnt_next;
                cnt2                        <= cnt2_next;
                src_is_reader               <= src_is_reader_next;
                locator_match               <= locator_match_next;
                builtin_endpoint            <= builtin_endpoint_next;
                is_metatraffic              <= is_metatraffic_next;
            end if;
        end if;
    end process;
    
end architecture;
