ipv6 flow stat on vm working

10 files changed, 298 insertions, 264 deletions

diff --git a/src/bp_sim.cpp b/src/bp_sim.cpp
index d2b75122..2d1d020b 100755
--- a/src/bp_sim.cpp
+++ b/src/bp_sim.cpp
@@ -6453,97 +6453,6 @@ void CFlowYamlDynamicPyloadPlugin::Dump(FILE *fd){
     }
 }
 
-uint16_t CSimplePacketParser::getPktSize(){
-    uint16_t ip_len=0;
-    if (m_ipv4) {
-        ip_len=m_ipv4->getTotalLength();
-    }
-    if (m_ipv6) {
-        ip_len=m_ipv6->getSize()+m_ipv6->getPayloadLen();
-    }
-    return ( ip_len +m_vlan_offset+14);
-}
-
-uint16_t CSimplePacketParser::getIpId() {
-    if (m_ipv4) {
-        return ( m_ipv4->getId() );
-    }
-
-    return (0);
-}
-
-uint8_t CSimplePacketParser::getTTl(){
-    if (m_ipv4) {
-        return ( m_ipv4->getTimeToLive() );
-    }
-    if (m_ipv6) {
-        return ( m_ipv6->getHopLimit() );
-    }
-    return (0);
-}
-
-bool CSimplePacketParser::Parse(){
-
-    rte_mbuf_t * m=m_m;
-    uint8_t *p=rte_pktmbuf_mtod(m, uint8_t*);
-    EthernetHeader *m_ether = (EthernetHeader *)p;
-    IPHeader * ipv4=0;
-    IPv6Header * ipv6=0;
-    m_vlan_offset=0;
-    m_option_offset=0;
-
-    uint8_t protocol = 0;
-
-    // Retrieve the protocol type from the packet
-    switch( m_ether->getNextProtocol() ) {
-    case EthernetHeader::Protocol::IP :
-        // IPv4 packet
-        ipv4=(IPHeader *)(p+14);
-        m_l4 = (uint8_t *)ipv4 + ipv4->getHeaderLength();
-        protocol = ipv4->getProtocol();
-        m_option_offset = 14 + IPV4_HDR_LEN;
-        break;
-    case EthernetHeader::Protocol::IPv6 :
-        // IPv6 packet
-        ipv6=(IPv6Header *)(p+14);
-        m_l4 = (uint8_t *)ipv6 + ipv6->getHeaderLength();
-        protocol = ipv6->getNextHdr();
-        m_option_offset = 14 +IPV6_HDR_LEN;
-        break;
-    case EthernetHeader::Protocol::VLAN :
-        m_vlan_offset = 4;
-        switch ( m_ether->getVlanProtocol() ){
-        case EthernetHeader::Protocol::IP:
-            // IPv4 packet
-            ipv4=(IPHeader *)(p+18);
-            m_l4 = (uint8_t *)ipv4 + ipv4->getHeaderLength();
-            protocol = ipv4->getProtocol();
-            m_option_offset = 18+ IPV4_HDR_LEN;
-            break;
-        case EthernetHeader::Protocol::IPv6 :
-            // IPv6 packet
-            ipv6=(IPv6Header *)(p+18);
-            m_l4 = (uint8_t *)ipv6 + ipv6->getHeaderLength();
-            protocol = ipv6->getNextHdr();
-            m_option_offset = 18 + IPV6_HDR_LEN;
-            break;
-        default:
-        break;
-        }
-        default:
-        break;
-    }
-    m_protocol =protocol;
-    m_ipv4=ipv4;
-    m_ipv6=ipv6;
-
-    if ( protocol == 0 ){
-        return (false);
-    }
-    return (true);
-}
-
-
 /* free the right object.
    it is classic to use virtual function but we can't do it here and we don't even want to use callback function
    as we want to save space and in most cases there is nothing to free.
diff --git a/src/flow_stat.cpp b/src/flow_stat.cpp
index 8c2f2566..a9451e17 100644
--- a/src/flow_stat.cpp
+++ b/src/flow_stat.cpp
@@ -63,7 +63,7 @@ stream_del: HW_ID_INIT
 static const uint16_t HW_ID_INIT = UINT16_MAX;
 static const uint16_t HW_ID_FREE = UINT16_MAX - 1;
 static const uint8_t PAYLOAD_RULE_PROTO = 255;
-const uint16_t FLOW_STAT_PAYLOAD_IP_ID = IP_ID_RESERVE_BASE + MAX_FLOW_STATS;
+const uint32_t FLOW_STAT_PAYLOAD_IP_ID = IP_ID_RESERVE_BASE + MAX_FLOW_STATS;
 
 inline std::string methodName(const std::string& prettyFunction)
 {
@@ -691,7 +691,7 @@ int CFlowStatRuleMgr::start_stream(TrexStream * stream) {
             return 0;
         }
 
-        uint16_t ip_id;
+        uint32_t ip_id;
         if (m_parser->get_ip_id(ip_id) < 0) {
             return 0; // if we could not find the ip id, no need to fix
         }
diff --git a/src/flow_stat.h b/src/flow_stat.h
index 25d16173..05b94f04 100644
--- a/src/flow_stat.h
+++ b/src/flow_stat.h
@@ -37,7 +37,7 @@
 #define IP_ID_RESERVE_BASE 0xff00
 #define FLOW_STAT_PAYLOAD_MAGIC 0xAB
 #define FLOW_STAT_PAYLOAD_INITIAL_FLOW_SEQ 0x01
-extern const uint16_t FLOW_STAT_PAYLOAD_IP_ID;
+extern const uint32_t FLOW_STAT_PAYLOAD_IP_ID;
 
 typedef std::map<uint32_t, uint16_t> flow_stat_map_t;
 typedef std::map<uint32_t, uint16_t>::iterator flow_stat_map_it_t;
diff --git a/src/flow_stat_parser.cpp b/src/flow_stat_parser.cpp
index 59de7481..602ef310 100644
--- a/src/flow_stat_parser.cpp
+++ b/src/flow_stat_parser.cpp
@@ -20,16 +20,19 @@
 */
 
 #include <netinet/in.h>
-#include <common/basic_utils.h>
-#include <common/Network/Packet/IPHeader.h>
-#include <common/Network/Packet/TcpHeader.h>
-#include <common/Network/Packet/IPv6Header.h>
-#include <common/Network/Packet/EthernetHeader.h>
-#include <flow_stat_parser.h>
+#include "common/basic_utils.h"
+#include "common/Network/Packet/EthernetHeader.h"
+#include "common/Network/Packet/IPHeader.h"
+#include "common/Network/Packet/IPv6Header.h"
+#include "common/Network/Packet/TcpHeader.h"
+#include "flow_stat_parser.h"
 
 void CFlowStatParser::reset() {
     m_ipv4 = 0;
+    m_ipv6 = 0;
     m_l4_proto = 0;
+    m_l4 = 0;
+    m_vlan_offset = 0;
     m_stat_supported = false;
 }
 
@@ -44,15 +47,32 @@ int CFlowStatParser::parse(uint8_t *p, uint16_t len) {
     switch( ether->getNextProtocol() ) {
     case EthernetHeader::Protocol::IP :
         m_ipv4 = (IPHeader *)(p + ETH_HDR_LEN);
+        m_l4 = ((uint8_t *)m_ipv4) + m_ipv4->getHeaderLength();
+        m_l4_proto = m_ipv4->getProtocol();
+        m_stat_supported = true;
+        break;
+    case EthernetHeader::Protocol::IPv6 :
+        m_ipv6 = (IPv6Header *)(p + ETH_HDR_LEN);
+        m_l4 = ((uint8_t *)m_ipv6) + m_ipv6->getHeaderLength();
+        m_l4_proto = m_ipv6->getNextHdr();
         m_stat_supported = true;
         break;
     case EthernetHeader::Protocol::VLAN :
+        m_vlan_offset = 4;
         min_len += 4;
         if (len < min_len)
             return -1;
         switch ( ether->getVlanProtocol() ){
         case EthernetHeader::Protocol::IP:
-            m_ipv4 = (IPHeader *)(p + 18);
+            m_ipv4 = (IPHeader *)(p + ETH_HDR_LEN + 4);
+            m_l4 = ((uint8_t *)m_ipv4) + m_ipv4->getHeaderLength();
+            m_l4_proto = m_ipv4->getProtocol();
+            m_stat_supported = true;
+            break;
+        case EthernetHeader::Protocol::IPv6 :
+            m_ipv6 = (IPv6Header *)(p + ETH_HDR_LEN + 4);
+            m_l4 = ((uint8_t *)m_ipv6) + m_ipv6->getHeaderLength();
+            m_l4_proto = m_ipv6->getNextHdr();
             m_stat_supported = true;
             break;
         default:
@@ -70,33 +90,68 @@ int CFlowStatParser::parse(uint8_t *p, uint16_t len) {
     return 0;
 }
 
-int CFlowStatParser::get_ip_id(uint16_t &ip_id) {
-    if (! m_ipv4)
-        return -1;
+int CFlowStatParser::get_ip_id(uint32_t &ip_id) {
+    if (m_ipv4) {
+        ip_id = m_ipv4->getId();
+        return 0;
+    }
 
-    ip_id = m_ipv4->getId();
+    if (m_ipv6) {
+        ip_id = m_ipv6->getFlowLabel();
+        return 0;
+    }
 
-    return 0;
+    return -1;
 }
 
-int CFlowStatParser::set_ip_id(uint16_t new_id) {
-    if (! m_ipv4)
-        return -1;
-
-    // Updating checksum, not recalculating, so if someone put bad checksum on purpose, it will stay bad
-    m_ipv4->updateCheckSum(PKT_NTOHS(m_ipv4->getId()), PKT_NTOHS(new_id));
-    m_ipv4->setId(new_id);
+int CFlowStatParser::set_ip_id(uint32_t new_id) {
+    if (m_ipv4) {
+        // Updating checksum, not recalculating, so if someone put bad checksum on purpose, it will stay bad
+        m_ipv4->updateCheckSum(PKT_NTOHS(m_ipv4->getId()), PKT_NTOHS(new_id));
+        m_ipv4->setId(new_id);
+        return 0;
+    }
 
-    return 0;
+    if (m_ipv6) {
+        m_ipv6->setFlowLabel(new_id);
+        return 0;
+    }
+    return -1;
 }
 
 int CFlowStatParser::get_l4_proto(uint8_t &proto) {
-    if (! m_ipv4)
-        return -1;
+    if (m_ipv4) {
+        proto = m_ipv4->getProtocol();
+        return 0;
+    }
 
-    proto = m_ipv4->getProtocol();
+    if (m_ipv6) {
+        proto = m_ipv6->getNextHdr();
+        return 0;
+    }
 
-    return 0;
+    return -1;
+}
+
+uint16_t CFlowStatParser::get_pkt_size() {
+    uint16_t ip_len=0;
+
+    if (m_ipv4) {
+        ip_len = m_ipv4->getTotalLength();
+    } else if (m_ipv6) {
+        ip_len = m_ipv6->getHeaderLength() + m_ipv6->getPayloadLen();
+    }
+    return ( ip_len + m_vlan_offset + ETH_HDR_LEN);
+}
+
+uint8_t CFlowStatParser::get_ttl(){
+    if (m_ipv4) {
+        return ( m_ipv4->getTimeToLive() );
+    }
+    if (m_ipv6) {
+        return ( m_ipv6->getHopLimit() );
+    }
+    return (0);
 }
 
 // calculate the payload len. Do not want to do this in parse(), since this is required only in
@@ -105,21 +160,33 @@ int CFlowStatParser::get_payload_len(uint8_t *p, uint16_t len, uint16_t &payload
     uint16_t l2_header_len;
     uint16_t l3_header_len;
     uint16_t l4_header_len;
+    uint8_t l4_proto = 0;
+    uint8_t *p_l3 = NULL;
     uint8_t *p_l4 = NULL;
     TCPHeader *p_tcp = NULL;
-    if (!m_ipv4) {
+    if (!m_ipv4 && !m_ipv6) {
         payload_len = 0;
         return -1;
     }
 
-    l2_header_len = ((uint8_t *)m_ipv4) - p;
-    l3_header_len = m_ipv4->getHeaderLength();
-    switch (m_ipv4->getProtocol()) {
+    if (m_ipv4) {
+        l2_header_len = ((uint8_t *)m_ipv4) - p;
+        l3_header_len = m_ipv4->getHeaderLength();
+        l4_proto = m_ipv4->getProtocol();
+        p_l3 = (uint8_t *)m_ipv4;
+    } else if (m_ipv6) {
+        l2_header_len = ((uint8_t *)m_ipv6) - p;
+        l3_header_len = IPV6_HDR_LEN;
+        l4_proto = m_ipv6->getNextHdr();
+        p_l3 = (uint8_t *)m_ipv6;
+    }
+
+    switch (l4_proto) {
     case IPPROTO_UDP:
         l4_header_len = 8;
         break;
     case IPPROTO_TCP:
-        p_l4 = ((uint8_t *)m_ipv4) + l3_header_len;
+        p_l4 = p_l3 + l3_header_len;
         if ((p_l4 + TCP_HEADER_LEN) > (p + len)) {
             //Not enough space for TCP header
             payload_len = 0;
@@ -129,8 +196,8 @@ int CFlowStatParser::get_payload_len(uint8_t *p, uint16_t len, uint16_t &payload
         l4_header_len = p_tcp->getHeaderLength();
         break;
     case IPPROTO_ICMP:
-    l4_header_len = 8;
-    break;
+        l4_header_len = 8;
+        break;
     default:
         l4_header_len = 0;
     }
@@ -149,7 +216,7 @@ static const uint16_t TEST_IP_ID = 0xabcd;
 static const uint8_t TEST_L4_PROTO = IPPROTO_UDP;
 
 int CFlowStatParser::test() {
-    uint16_t ip_id = 0;
+    uint32_t ip_id = 0;
     uint8_t l4_proto;
     uint8_t test_pkt[] = {
         // ether header
@@ -263,3 +330,58 @@ int C82599Parser::parse(uint8_t *p, uint16_t len) {
 
     return 0;
 }
+
+bool CSimplePacketParser::Parse(){
+
+    rte_mbuf_t * m=m_m;
+    uint8_t *p=rte_pktmbuf_mtod(m, uint8_t*);
+    EthernetHeader *m_ether = (EthernetHeader *)p;
+    IPHeader * ipv4=0;
+    IPv6Header * ipv6=0;
+    m_vlan_offset=0;
+    uint8_t protocol = 0;
+
+    // Retrieve the protocol type from the packet
+    switch( m_ether->getNextProtocol() ) {
+    case EthernetHeader::Protocol::IP :
+        // IPv4 packet
+        ipv4=(IPHeader *)(p+14);
+        m_l4 = (uint8_t *)ipv4 + ipv4->getHeaderLength();
+        protocol = ipv4->getProtocol();
+        break;
+    case EthernetHeader::Protocol::IPv6 :
+        // IPv6 packet
+        ipv6=(IPv6Header *)(p+14);
+        m_l4 = (uint8_t *)ipv6 + ipv6->getHeaderLength();
+        protocol = ipv6->getNextHdr();
+        break;
+    case EthernetHeader::Protocol::VLAN :
+        m_vlan_offset = 4;
+        switch ( m_ether->getVlanProtocol() ){
+        case EthernetHeader::Protocol::IP:
+            // IPv4 packet
+            ipv4=(IPHeader *)(p+18);
+            m_l4 = (uint8_t *)ipv4 + ipv4->getHeaderLength();
+            protocol = ipv4->getProtocol();
+            break;
+        case EthernetHeader::Protocol::IPv6 :
+            // IPv6 packet
+            ipv6=(IPv6Header *)(p+18);
+            m_l4 = (uint8_t *)ipv6 + ipv6->getHeaderLength();
+            protocol = ipv6->getNextHdr();
+            break;
+        default:
+        break;
+        }
+        default:
+        break;
+    }
+    m_protocol =protocol;
+    m_ipv4=ipv4;
+    m_ipv6=ipv6;
+
+    if ( protocol == 0 ){
+        return (false);
+    }
+    return (true);
+}
diff --git a/src/flow_stat_parser.h b/src/flow_stat_parser.h
index b4f01900..d44c8a7f 100644
--- a/src/flow_stat_parser.h
+++ b/src/flow_stat_parser.h
@@ -22,27 +22,65 @@
 #ifndef __FLOW_STAT_PARSER_H__
 #define __FLOW_STAT_PARSER_H__
 
-// Basic flow stat parser. Relevant for xl710/x710/x350 cards
+#include <arpa/inet.h>
 #include "common/Network/Packet/IPHeader.h"
+#include "common/Network/Packet/IPv6Header.h"
+#include "common/Network/Packet/TcpHeader.h"
+#include "mbuf.h"
 
+// Basic flow stat parser. Relevant for xl710/x710/x350 cards
 class CFlowStatParser {
  public:
     virtual ~CFlowStatParser() {}
     virtual void reset();
     virtual int parse(uint8_t *pkt, uint16_t len);
     virtual bool is_stat_supported() {return m_stat_supported == true;}
-    virtual int get_ip_id(uint16_t &ip_id);
-    virtual int set_ip_id(uint16_t ip_id);
+    virtual int get_ip_id(uint32_t &ip_id);
+    virtual int set_ip_id(uint32_t ip_id);
     virtual int get_l4_proto(uint8_t &proto);
     virtual int get_payload_len(uint8_t *p, uint16_t len, uint16_t &payload_len);
+    virtual uint16_t get_pkt_size();
+    virtual uint8_t get_ttl();
     virtual int test();
+    uint8_t *get_l3() {
+        if (m_ipv4)
+            return (uint8_t *)m_ipv4;
+        else
+            return (uint8_t *)m_ipv6;
+    }
+    uint8_t * get_l4() {return m_l4;}
+
+    inline bool IsNatInfoPkt(bool &first) {
+        if (!m_ipv4 || (m_l4_proto != IPPROTO_TCP)) {
+            return false;
+        }
+        if ((m_l4 + TCP_HEADER_LEN) > (uint8_t *)m_ipv4 + get_pkt_size()) {
+            return false;
+        }
+        // If we are here, relevant fields from tcp header are inside the packet boundaries
+        // We want to handle SYN and SYN+ACK packets
+        TCPHeader *tcp = (TCPHeader *)m_l4;
+        if (! tcp->getSynFlag())
+            return false;
+
+        if (! tcp->getAckFlag()) {
+            first = true;
+        } else {
+            first = false;
+        }
+        return true;
+    }
 
  protected:
     IPHeader *m_ipv4;
+    IPv6Header *m_ipv6;
+    uint8_t *m_l4;
     bool m_stat_supported;
     uint8_t m_l4_proto;
+    uint8_t m_vlan_offset;
 };
 
+// relevant for 82599 card
 class C82599Parser : public CFlowStatParser {
  public:
     C82599Parser(bool vlan_supported) {m_vlan_supported = vlan_supported;}
@@ -53,4 +91,47 @@ class C82599Parser : public CFlowStatParser {
     bool m_vlan_supported;
 };
 
+// Used for latency statefull packets. Need to be merged with above parser
+class CSimplePacketParser {
+ public:
+    CSimplePacketParser(rte_mbuf_t * m){
+        m_m = m;
+        m_l4 = NULL;
+    }
+    bool Parse();
+
+    // Check if this packet contains NAT info in TCP ack
+    // first - set to true if this is the first packet of the flow. false otherwise.
+    //         relevant only if return value is true
+    inline bool IsNatInfoPkt(bool &first) {
+        if (!m_ipv4 || (m_protocol != IPPROTO_TCP)) {
+            return false;
+        }
+        if (! m_l4 || (m_l4 - rte_pktmbuf_mtod(m_m, uint8_t*) + TCP_HEADER_LEN) > m_m->data_len) {
+            return false;
+        }
+        // If we are here, relevant fields from tcp header are guaranteed to be in first mbuf
+        // We want to handle SYN and SYN+ACK packets
+        TCPHeader *tcp = (TCPHeader *)m_l4;
+        if (! tcp->getSynFlag())
+            return false;
+
+        if (! tcp->getAckFlag()) {
+            first = true;
+        } else {
+            first = false;
+        }
+        return true;
+    }
+
+ public:
+    IPHeader *      m_ipv4;
+    IPv6Header *    m_ipv6;
+    uint8_t         m_protocol;
+    uint16_t        m_vlan_offset;
+    uint8_t *       m_l4;
+ private:
+    rte_mbuf_t *    m_m ;
+};
+
 #endif
diff --git a/src/latency.cpp b/src/latency.cpp
index 768e161b..03c48a25 100644
--- a/src/latency.cpp
+++ b/src/latency.cpp
@@ -21,6 +21,7 @@ limitations under the License.
 */
 #include "latency.h"
 #include "bp_sim.h"
+#include "flow_stat_parser.h"
 #include "utl_json.h"
 #include "trex_watchdog.h"
 
@@ -371,19 +372,12 @@ bool CCPortLatency::check_packet(rte_mbuf_t * m,CRx_check_header * & rx_p) {
     }    
     CLatencyPktMode *c_l_pkt_mode = m_parent->c_l_pkt_mode;
     uint16_t pkt_size=rte_pktmbuf_pkt_len(m);
-
-    /* check if CRC was extracted */
-    if ( parser.getPktSize() == pkt_size-4) {
-        // CRC was not extracted by driver (VM E1000 driver issue) extract it
-        pkt_size=pkt_size-4;
-    }
-
     uint16_t vlan_offset=parser.m_vlan_offset;
     uint8_t *p=rte_pktmbuf_mtod(m, uint8_t*);
 
     rx_p = (CRx_check_header *)0;
 
-    bool is_lateancy_pkt =  c_l_pkt_mode->IsLatencyPkt(parser.m_ipv4) & parser.IsLatencyPkt(parser.m_l4 + c_l_pkt_mode->l4_header_len());
+    bool is_lateancy_pkt =  c_l_pkt_mode->IsLatencyPkt(parser.m_ipv4) & IsLatencyPkt(parser.m_l4 + c_l_pkt_mode->l4_header_len());
 
     if ( ! is_lateancy_pkt) {
 
diff --git a/src/latency.h b/src/latency.h
index 63e50337..e398d7c7 100644
--- a/src/latency.h
+++ b/src/latency.h
@@ -21,8 +21,8 @@ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 See the License for the specific language governing permissions and
 limitations under the License.
 */
-#include <bp_sim.h>
-#include <flow_stat.h>
+#include "bp_sim.h"
+#include "flow_stat.h"
 
 #define L_PKT_SUBMODE_NO_REPLY 1
 #define L_PKT_SUBMODE_REPLY 2
@@ -59,18 +59,15 @@ private:
     ipaddr_t            m_client_ip;
     ipaddr_t            m_server_ip;
     uint32_t            m_dual_port_mask;
-
     CGenNode            m_dummy_node;
     CFlowPktInfo        m_pkt_info;
     CPacketIndication   m_pkt_indication;
     CCapPktRaw *        m_packet;
 };
 
-
 #define LATENCY_MAGIC 0x12345600
 
 struct  latency_header {
-
     uint64_t time_stamp;
     uint32_t magic;
     uint32_t seq;
@@ -80,72 +77,9 @@ struct  latency_header {
     }
 };
 
-class CSimplePacketParser {
-public:
-
-    CSimplePacketParser(rte_mbuf_t * m){
-        m_m=m;
-        m_l4 = NULL;
-    }
-
-    bool Parse();
-    uint8_t getTTl();
-    uint16_t getIpId();
-    uint16_t getPktSize();
-
-    // Check if packet contains latency data
-    inline bool IsLatencyPkt(uint8_t *p) {
-	if (! p)
-	    return false;
-
-        latency_header * h=(latency_header *)(p);
-        if ( (h->magic & 0xffffff00) != LATENCY_MAGIC ){
-            return false;
-        }
-
-        return true;
-    }
-
-    // Check if this packet contains NAT info in TCP ack
-    // first - set to true if this is the first packet of the flow. false otherwise.
-    //         relevant only if return value is true
-    inline bool IsNatInfoPkt(bool &first) {
-        if (!m_ipv4 || (m_protocol != IPPROTO_TCP)) {
-            return false;
-        }
-        if (! m_l4 || (m_l4 - rte_pktmbuf_mtod(m_m, uint8_t*) + TCP_HEADER_LEN) > m_m->data_len) {
-            return false;
-        }
-        // If we are here, relevant fields from tcp header are guaranteed to be in first mbuf
-        // We want to handle SYN and SYN+ACK packets
-        TCPHeader *tcp = (TCPHeader *)m_l4;
-        if (! tcp->getSynFlag())
-            return false;
-
-        if (! tcp->getAckFlag()) {
-            first = true;
-        } else {
-            first = false;
-        }
-        return true;
-    }
-
-public:
-    IPHeader *      m_ipv4;
-    IPv6Header *    m_ipv6;
-    uint8_t         m_protocol;
-    uint16_t        m_vlan_offset;
-    uint16_t        m_option_offset;
-    uint8_t *       m_l4;
-private: 
-    rte_mbuf_t *    m_m ;
-};
-
-
-
 class CLatencyManager ;
 
-// per port 
+// per port
 class CCPortLatency {
 public:
     bool Create(CLatencyManager * parent,
@@ -165,7 +99,7 @@ public:
                 return(true);
             else
                 return(false);
-        }        
+        }
 
         if ( !CGlobalInfo::is_learn_mode() ) {
             return(true);
@@ -175,34 +109,23 @@ public:
     uint32_t external_nat_ip(){
         return (m_nat_external_ip);
     }
-
     void update_packet(rte_mbuf_t * m, int port_id);
-
     bool do_learn(uint32_t external_ip);
-
-    bool check_packet(rte_mbuf_t * m,
-                      CRx_check_header * & rx_p);
+    bool check_packet(rte_mbuf_t * m, CRx_check_header * & rx_p);
     bool check_rx_check(rte_mbuf_t * m);
-
-
-	bool dump_packet(rte_mbuf_t * m);
-
+    bool dump_packet(rte_mbuf_t * m);
     void DumpCounters(FILE *fd);
     void dump_counters_json(std::string & json );
-
     void DumpShort(FILE *fd);
     void dump_json(std::string & json );
     void dump_json_v2(std::string & json );
-
     uint32_t get_jitter_usec(void){
         return ((uint32_t)(m_jitter.get_jitter()*1000000.0));
     }
-
-
     static void DumpShortHeader(FILE *fd);
 
     bool is_any_err(){
-        if (  (m_tx_pkt_ok == m_rx_port->m_pkt_ok ) && 
+        if (  (m_tx_pkt_ok == m_rx_port->m_pkt_ok ) &&
 
               ((m_unsup_prot+
                 m_no_magic+
@@ -217,25 +140,35 @@ public:
     uint16_t get_icmp_tx_seq() {return m_icmp_tx_seq;}
     uint16_t get_icmp_rx_seq() {return m_icmp_rx_seq;}
 
+    // Check if packet contains latency data
+    static inline bool IsLatencyPkt(uint8_t *p) {
+        if (! p)
+            return false;
+
+        latency_header * h=(latency_header *)(p);
+        if ( (h->magic & 0xffffff00) != LATENCY_MAGIC ){
+            return false;
+        }
+
+        return true;
+    }
+
 private:
     std::string get_field(std::string name,float f);
 
-    
+
 private:
      CLatencyManager * m_parent;
      CCPortLatency *   m_rx_port; /* corespond rx port  */
-     bool              m_nat_learn;  
+     bool              m_nat_learn;
      bool              m_nat_can_send;
      uint32_t          m_nat_external_ip;
-
      uint32_t m_tx_seq;
      uint32_t m_rx_seq;
-
      uint8_t  m_pad;
      uint8_t  m_id;
      uint16_t m_payload_offset;
      uint16_t m_l4_offset;
-
      uint16_t m_pkt_size;
      // following two variables are for the latency ICMP reply mode.
      // if we want to pass through firewall, we want to send reply only after we got request with same seq num
@@ -248,7 +181,6 @@ private:
 public:
      uint64_t m_tx_pkt_ok;
      uint64_t m_tx_pkt_err;
-
      uint64_t m_pkt_ok;
      uint64_t m_unsup_prot;
      uint64_t m_no_magic;
@@ -263,10 +195,10 @@ public:
 
 
 class CPortLatencyHWBase {
-public:
+ public:
     virtual int tx(rte_mbuf_t * m)=0;
     virtual rte_mbuf_t * rx()=0;
-    virtual uint16_t rx_burst(struct rte_mbuf **rx_pkts, 
+    virtual uint16_t rx_burst(struct rte_mbuf **rx_pkts,
                                uint16_t nb_pkts){
         return(0);
     }
@@ -274,7 +206,7 @@ public:
 
 
 class CLatencyManagerCfg {
-public:
+ public:
     CLatencyManagerCfg (){
         m_max_ports=0;
         m_cps=0.0;
@@ -283,25 +215,23 @@ public:
         m_server_ip.v4=0x20000000;
         m_dual_port_mask=0x01000000;
     }
+
+ public:
     uint32_t             m_max_ports;
     double               m_cps;// CPS
     CPortLatencyHWBase * m_ports[TREX_MAX_PORTS];
     ipaddr_t             m_client_ip;
     ipaddr_t             m_server_ip;
     uint32_t             m_dual_port_mask;
-
 };
 
-
 class CLatencyManagerPerPort {
 public:
      CCPortLatency          m_port;
      CPortLatencyHWBase  *  m_io;
      uint32_t               m_flag;
-
 };
 
-
 class CLatencyPktMode {
  public:
     uint8_t m_submode;
@@ -358,10 +288,10 @@ public:
         m_pkt_gen.set_ip(client_ip,server_ip,mask_dual_port);
     }
     void Dump(FILE *fd); // dump all
-    void DumpShort(FILE *fd); // dump short histogram of latency 
+    void DumpShort(FILE *fd); // dump short histogram of latency
 
     void DumpRxCheck(FILE *fd); // dump all
-    void DumpShortRxCheck(FILE *fd); // dump short histogram of latency 
+    void DumpShortRxCheck(FILE *fd); // dump short histogram of latency
     void dump_nat_flow_table(FILE *fd);
     void rx_check_dump_json(std::string & json);
     uint16_t get_latency_header_offset(){
@@ -370,7 +300,7 @@ public:
     void update();
     void update_fast();
 
-    void dump_json(std::string & json ); // dump to json 
+    void dump_json(std::string & json ); // dump to json
     void dump_json_v2(std::string & json );
     void DumpRxCheckVerification(FILE *fd,uint64_t total_tx_rx_check);
     void set_mask(uint32_t mask){
@@ -391,26 +321,24 @@ private:
     void  send_pkt_all_ports();
     void  try_rx();
     void  try_rx_queues();
-    void  run_rx_queue_msgs(uint8_t thread_id,
-                                             CNodeRing * r);
-	void  wait_for_rx_dump();
-    void  handle_rx_pkt(CLatencyManagerPerPort * lp,
-                        rte_mbuf_t * m);
+    void  run_rx_queue_msgs(uint8_t thread_id, CNodeRing * r);
+    void  wait_for_rx_dump();
+    void  handle_rx_pkt(CLatencyManagerPerPort * lp, rte_mbuf_t * m);
     /* messages handlers */
-    void handle_latency_pkt_msg(uint8_t thread_id,
-                               CGenNodeLatencyPktInfo * msg);
+    void handle_latency_pkt_msg(uint8_t thread_id, CGenNodeLatencyPktInfo * msg);
 
+ private:
      pqueue_t                m_p_queue; /* priorty queue */
      bool                    m_is_active;
      CLatencyPktInfo         m_pkt_gen;
      CLatencyManagerPerPort  m_ports[TREX_MAX_PORTS];
-     uint64_t                m_d_time; // calc tick betwen sending 
+     uint64_t                m_d_time; // calc tick betwen sending
      double                  m_cps;
      double                  m_delta_sec;
-     uint64_t                m_start_time; // calc tick betwen sending 
+     uint64_t                m_start_time; // calc tick betwen sending
      uint32_t                m_port_mask;
      uint32_t                m_max_ports;
-     RxCheckManager 	     m_rx_check_manager;
+     RxCheckManager          m_rx_check_manager;
      CNatRxManager           m_nat_check_manager;
      CCpuUtlDp               m_cpu_dp_u;
      CCpuUtlCp               m_cpu_cp_u;
@@ -420,4 +348,3 @@ private:
 };
 
 #endif
-
diff --git a/src/main_dpdk.cpp b/src/main_dpdk.cpp
index b961f653..be3863eb 100644
--- a/src/main_dpdk.cpp
+++ b/src/main_dpdk.cpp
@@ -4519,35 +4519,36 @@ int CPhyEthIF::get_flow_stats_payload(rx_per_flow_t *rx_stats, tx_per_flow_t *tx
 
 // If needed, send packets to rx core for processing.
 // This is relevant only in VM case, where we receive packets to the working DP core (only 1 DP core in this case)
-bool CCoreEthIF::process_rx_pkt(pkt_dir_t   dir,
-                                rte_mbuf_t * m){
+bool CCoreEthIF::process_rx_pkt(pkt_dir_t dir, rte_mbuf_t * m) {
+    CFlowStatParser parser;
+    uint32_t ip_id;
 
-    CSimplePacketParser parser(m);
-    if ( !parser.Parse()  ){
+    if (parser.parse(rte_pktmbuf_mtod(m, uint8_t*), rte_pktmbuf_pkt_len(m)) != 0) {
         return false;
     }
     bool send=false;
 
     // e1000 on ESXI hands us the packet with the ethernet FCS
-    if (parser.getPktSize() < m->pkt_len) {
-        rte_pktmbuf_trim(m, m->pkt_len - parser.getPktSize());
+    if (parser.get_pkt_size() < rte_pktmbuf_pkt_len(m)) {
+        rte_pktmbuf_trim(m, rte_pktmbuf_pkt_len(m) - parser.get_pkt_size());
     }
 
     if ( get_is_stateless() ) {
         // In stateless RX, we only care about flow stat packets
-        if ((parser.getIpId() & 0xff00) == IP_ID_RESERVE_BASE) {
+        if ((parser.get_ip_id(ip_id) == 0) && ((ip_id & 0xff00) == IP_ID_RESERVE_BASE)) {
             send = true;
         }
     } else {
         CLatencyPktMode *c_l_pkt_mode = g_trex.m_mg.c_l_pkt_mode;
-        bool is_lateancy_pkt =  c_l_pkt_mode->IsLatencyPkt(parser.m_ipv4) & parser.IsLatencyPkt(parser.m_l4 + c_l_pkt_mode->l4_header_len());
+        bool is_lateancy_pkt =  c_l_pkt_mode->IsLatencyPkt((IPHeader *)parser.get_l4()) &
+            CCPortLatency::IsLatencyPkt(parser.get_l4() + c_l_pkt_mode->l4_header_len());
 
         if (is_lateancy_pkt) {
             send = true;
         } else {
             if ( get_is_rx_filter_enable() ) {
                 uint8_t max_ttl = 0xff - get_rx_check_hops();
-                uint8_t pkt_ttl = parser.getTTl();
+                uint8_t pkt_ttl = parser.get_ttl();
                 if ( (pkt_ttl==max_ttl) || (pkt_ttl==(max_ttl-1) ) ) {
                     send=true;
                 }
@@ -5637,7 +5638,7 @@ int CTRexExtendedDriverBase40G::configure_rx_filter_rules_statfull(CPhyEthIF * _
 
 const uint32_t FDIR_TEMP_HW_ID = 511;
 const uint32_t FDIR_PAYLOAD_RULES_HW_ID = 510;
-extern const uint16_t FLOW_STAT_PAYLOAD_IP_ID;
+extern const uint32_t FLOW_STAT_PAYLOAD_IP_ID;
 int CTRexExtendedDriverBase40G::configure_rx_filter_rules(CPhyEthIF * _if) {
     if (get_is_stateless()) {
         uint32_t port_id = _if->get_port_id();
diff --git a/src/stateless/rx/trex_stateless_rx_core.cpp b/src/stateless/rx/trex_stateless_rx_core.cpp
index 853fc868..577f5992 100644
--- a/src/stateless/rx/trex_stateless_rx_core.cpp
+++ b/src/stateless/rx/trex_stateless_rx_core.cpp
@@ -191,7 +191,7 @@ void CRxCoreStateless::handle_rx_pkt(CLatencyManagerPerPortStl *lp, rte_mbuf_t *
     CFlowStatParser parser;
 
     if (parser.parse(rte_pktmbuf_mtod(m, uint8_t *), m->pkt_len) == 0) {
-        uint16_t ip_id;
+        uint32_t ip_id;
         if (parser.get_ip_id(ip_id) == 0) {
             if (is_flow_stat_id(ip_id)) {
                 uint16_t hw_id;
@@ -387,12 +387,12 @@ int CRxCoreStateless::try_rx() {
     return total_pkts;
 }
 
-bool CRxCoreStateless::is_flow_stat_id(uint16_t id) {
-    if ((id & 0xff00) == IP_ID_RESERVE_BASE) return true;
+bool CRxCoreStateless::is_flow_stat_id(uint32_t id) {
+    if ((id & 0x000fff00) == IP_ID_RESERVE_BASE) return true;
     return false;
 }
 
-bool CRxCoreStateless::is_flow_stat_payload_id(uint16_t id) {
+bool CRxCoreStateless::is_flow_stat_payload_id(uint32_t id) {
     if (id == FLOW_STAT_PAYLOAD_IP_ID) return true;
     return false;
 }
diff --git a/src/stateless/rx/trex_stateless_rx_core.h b/src/stateless/rx/trex_stateless_rx_core.h
index fc66704e..e86e0eea 100644
--- a/src/stateless/rx/trex_stateless_rx_core.h
+++ b/src/stateless/rx/trex_stateless_rx_core.h
@@ -151,8 +151,8 @@ class CRxCoreStateless {
     void flush_rx();
     int try_rx();
     void try_rx_queues();
-    bool is_flow_stat_id(uint16_t id);
-    bool is_flow_stat_payload_id(uint16_t id);
+    bool is_flow_stat_id(uint32_t id);
+    bool is_flow_stat_payload_id(uint32_t id);
     uint16_t get_hw_id(uint16_t id);
 
  private: