diff --git a/src/TODO.txt b/src/TODO.txt
index fd56659..7531d65 100644
--- a/src/TODO.txt
+++ b/src/TODO.txt
@@ -65,7 +65,6 @@ ENDPOINT FIFO PACKET FORMAT
 
 32..............24..............16..............8...............0
 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
-|                            LENTGH                             |
 +---------------+---------------+---------------+---------------+
 |     OPCODE    |     FLAGS     |         SRC_UDP_PORT          |
 +---------------+---------------+---------------+---------------+
diff --git a/src/rtps_builtin_endpoint.vhd b/src/rtps_builtin_endpoint.vhd
index f2d504e..eec1f1c 100644
--- a/src/rtps_builtin_endpoint.vhd
+++ b/src/rtps_builtin_endpoint.vhd
@@ -18,6 +18,7 @@ entity rtps_builtin_endpoint is
         empty           : in std_logic;                                             -- Input FIFO empty flag
         rd              : out std_logic;                                            -- Input FIFO read signal
         data_in         : in std_logic_vector(31 downto 0);                         -- Input FIFO data signal
+        last_word_in    : in std_logic;
         endpoint_output : out USER_ENDPOINT_OUTPUT;
         endpoint_full   : in std_logic_vector(0 to MAX_ENDPOINTS-1);
         endpoint_wr     : out std_logic_vector(0 to MAX_ENDPOINTS-1);
@@ -122,6 +123,7 @@ architecture arch of rtps_builtin_endpoint is
     signal reset_max_pointer, reset_max_pointer_next : std_logic := '0';
     signal stale_check, stale_check_next : std_logic := '0';
     signal mem_guidprefix, mem_guidprefix_next : GUIDPREFIX_ARRAY_TYPE : (others => (others => '0'));
+    signal last_word_in_latch, last_word_in_latch_next : std_logic := '0';
     
     
     --*****ALIAS DEFINATION*****
@@ -285,6 +287,14 @@ begin
         wr_sig                          <= '0';
         stale_check_next                <= stale_check;
         
+        -- Last Word Latch Setter
+        if (last_word_in = '1') then
+            last_word_in_latch_next <= '1';
+            -- TODO: Also set in AUTO_PURGE to exit from SKIP_PACKET stage
+        else
+            last_word_in_latch_next <= last_word_in_latch;
+        end if;
+        
         -- TODO: Reset Latches
         
         case(stage) is
@@ -304,16 +314,6 @@ begin
                 else
                     -- Process Packet
                     stale_check_next    <= '0';
-                    stage_next          <= PACKET_LENGTH;
-                end if;
-            when PACKET_LENGTH =>
-                if (empty = '0') then
-                    rd_sig              <= '1';
-                    -- Latch Packet Length
-                    packet_length_next  <= unsigned(data_in(13 downto 0));
-                    -- Reset Word Counter
-                    reset_read_cnt      <= '1';
-                    -- Next Stage
                     stage_next          <= PACKET_HEADER;
                 end if;
             when PACKET_HEADER =>
@@ -600,9 +600,11 @@ begin
                 end if;
             when PROCESS_PL =>
                 if (empty = '0') then
-                    rd_sig                  <= '1';
+                    rd_sig              <= '1';
+                    -- Reset Word Counter
+                    reset_read_cnt      <= '1';
                     -- Latch Parameter End
-                    parameter_end_next  <= read_cnt + unsigned(normalize_length(endian_swap(endian_flag,parameter_length)));
+                    parameter_end_next  <= unsigned(normalize_length(endian_swap(endian_flag,parameter_length)));
                     -- DEFAULT      
                     stage_next          <= SKIP_PARAMETER;
                     -- TODO: Use inline QoS?
@@ -1471,13 +1473,13 @@ begin
                     end if;
                 end if;
             when SKIP_PACKET =>
-                if (read_cnt > packet_length) then
+                if (last_word_in_latch = '1') then
                     -- Begin parsing of next parameter
-                    stage_next  <= PROCESS_PL;
+                    stage_next  <= IDLE;
                 elsif (empty = '0') then
                     rd_sig  <= '1';
                     -- End of Packet
-                    if (read_cnt = packet_length) then
+                    if (last_word_in = '1') then
                         -- Continue parsing next packet
                         stage_next  <= IDLE;
                     end if;
diff --git a/src/rtps_handler.vhd b/src/rtps_handler.vhd
index e5ff67a..a61e50d 100644
--- a/src/rtps_handler.vhd
+++ b/src/rtps_handler.vhd
@@ -7,7 +7,7 @@ use work.rtps_package.all;
 
 -- TODO: Remove alignment logic for RTPS Submessages, since all Submessages are 32-bit aligned
 -- TODO: Merge CHECK_SUB_END and SKIP_SUB stages
--- TODO: Fix payload length
+-- TODO: Remove payload length and implement "last_word" bit
 -- TODO: Change all Endpoint Bit Vectors to "to" Ranges
 -- Checksum has to be checked before
 
@@ -210,7 +210,7 @@ architecture arch of rtps_handler is
         end loop;
         return id;
     end function;
-    function match_id_domain (  ref : std_logic_vector(ENTITYID_WIDTH-1 downto 0);
+    function match_id_endpoint (  ref : std_logic_vector(ENTITYID_WIDTH-1 downto 0);
                                 ar : ENTITYID_TYPE) return integer is
         variable id : integer := 0;
     begin
@@ -901,7 +901,7 @@ begin
                         builtin_endpoint_next   <= '1';
                     -- Match User Entity ID
                     elsif (is_metatraffic = '0') then
-                        tmp := match_id_domain(aligned_data_in, ENTITYID);
+                        tmp := match_id_endpoint(aligned_data_in, ENTITYID);
                         -- Entity non existent, skip Submessage
                         if (tmp = MAX_ENDPOINTS) then
                             stage_next  <= SKIP_SUB;