Added ICMP latency packet modes

9 files changed, 1885 insertions, 1473 deletions

diff --git a/src/bp_gtest.cpp b/src/bp_gtest.cpp
index a94c2d37..3ef25c9d 100755
--- a/src/bp_gtest.cpp
+++ b/src/bp_gtest.cpp
@@ -31,6 +31,7 @@ limitations under the License.
 #include "msg_manager.h"
 #include <common/cgen_map.h>
 #include "platform_cfg.h"
+#include "latency.h"
 
 int test_policer(){
     CPolicer policer;
@@ -623,7 +624,6 @@ TEST_F(basic, test_pcap_mode1) {
      EXPECT_EQ_UINT32(1, res?1:0)<< "pass";
 }
 
-
 /* check override the low IPG */
 TEST_F(basic, test_pcap_mode2) {
 
@@ -638,154 +638,191 @@ TEST_F(basic, test_pcap_mode2) {
      EXPECT_EQ_UINT32(1, res?1:0)<< "pass";
 }
 
+#define l_pkt_test_s_ip 0x01020304
+#define l_pkt_test_d_ip 0x05060708
 
-
-
-
-TEST_F(basic, latency1) {
-    CLatencyPktInfo l;
-    l.Create();
-
-    CParserOption * po =&CGlobalInfo::m_options;
-    po->preview.setVMode(3);
-    po->preview.setFileWrite(true);
-
-    uint8_t  mac[]={0,0,0,1,0,0};
-    (void)mac;
-
-    CErfIF  erf_vif;
-    erf_vif.set_review_mode(&CGlobalInfo::m_options.preview);
-
-    erf_vif.open_file("exp/sctp.erf");
-
-
-    mac[0]=0;
-    mac[1]=0;
-    mac[2]=0;
-    mac[3]=1;
-    mac[4]=0;
-    mac[5]=0;
-
-
-    l.set_ip(0x10000000,0x20000000,0x01000000);
-
-    int i;
-    /* simulate 8 ports */
-    for (i=0;i<8; i++) {
-        rte_mbuf_t * m=l.generate_pkt(i);
-        erf_vif.send_node(l.getNode());
-        rte_pktmbuf_free(m);
-    }
-
-    erf_vif.close_file();
-
-
-    l.Delete();
-
-    CErfCmp cmp;
-    cmp.dump=1;
-
-    bool res=true;
-
-    if ( cmp.compare("exp/sctp.erf","exp/sctp-ex.erf") != true ) {
-        res=false;
+CLatencyPktMode *
+latency_pkt_mod_create(uint8_t l_pkt_mode) {
+    switch (l_pkt_mode) {
+    default:
+        return  new CLatencyPktModeSCTP(l_pkt_mode);
+        break;
+    case L_PKT_SUBMODE_NO_REPLY:
+    case L_PKT_SUBMODE_REPLY:
+    case L_PKT_SUBMODE_0_SEQ:
+        return new CLatencyPktModeICMP(l_pkt_mode);
+        break;
     }
-    EXPECT_EQ_UINT32((uint32_t)res?1:0, (uint32_t)1)<< "pass";
 }
 
-
-
-TEST_F(basic, latency2) {
-    CLatencyPktInfo l;
+rte_mbuf_t *
+create_latency_test_pkt(uint8_t l_pkt_mode, uint16_t &pkt_size, uint8_t port_id, uint8_t pkt_num) {
+    rte_mbuf_t * m;
+    CLatencyPktMode *c_l_pkt_mode;
     CCPortLatency port0;
-    l.Create();
-    port0.Create(0,0,l.get_payload_offset(),l.get_pkt_size(),0);
-
-
-    uint8_t  mac[]={0,0,0,1,0,0};
-    (void)mac;
-
-    mac[0]=0;
-    mac[1]=0;
-    mac[2]=0;
-    mac[3]=1;
-    mac[4]=0;
-    mac[5]=0;
-
-
-    l.set_ip(0x01000000,0x02000000,0x01000000);
-
-
-    int i;
-    for (i=0; i<100; i++) {
-        rte_mbuf_t * m=l.generate_pkt(0);
-        rte_pktmbuf_mtod(m, uint8_t*);
-        //utl_DumpBuffer(stdout,p,l.get_pkt_size(),0);
-
-        port0.update_packet(m);
+    CLatencyPktInfo info;
+    CLatencyManager mgr;
 
-        rte_pktmbuf_mtod(m, uint8_t*);
-        //utl_DumpBuffer(stdout,p,l.get_pkt_size(),0);
-        //printf("offset is : %d \n",l.get_payload_offset());
+    c_l_pkt_mode = latency_pkt_mod_create(l_pkt_mode);
 
-        CRx_check_header *  rx_p;
-        bool res=port0.check_packet(m,rx_p);
-        EXPECT_EQ_UINT32((uint32_t)res?1:0, (uint32_t)1)<< "pass";
-        if (!res ) {
-            printf(" ERROR \n");
+    mgr.c_l_pkt_mode = c_l_pkt_mode;
+    info.Create(c_l_pkt_mode);
+    port0.Create(&mgr, 0, info.get_payload_offset(), info.get_l4_offset(), info.get_pkt_size(), 0);
+    info.set_ip(l_pkt_test_s_ip ,l_pkt_test_d_ip, 0x01000000);
+    m=info.generate_pkt(0);
+    while (pkt_num > 0) {
+        pkt_num--;
+        port0.update_packet(m, port_id);
+    }
+    pkt_size = info.get_pkt_size();
+    port0.Delete();
+    info.Delete();
+    delete c_l_pkt_mode;
+
+    return m;
+}
+
+bool
+verify_latency_pkt(uint8_t *p, uint8_t proto, uint16_t icmp_seq, uint8_t icmp_type) {
+    EthernetHeader *eth = (EthernetHeader *)p;
+    IPHeader *ip = (IPHeader *)(p + 14);
+    uint8_t  srcmac[]={0,0,0,1,0,0};    
+    uint8_t  dstmac[]={0,0,0,1,0,0};
+    latency_header * h;
+ 
+    // eth
+    EXPECT_EQ_UINT32(eth->getNextProtocol(), 0x0800)<< "Failed ethernet next protocol check";
+    EXPECT_EQ_UINT32(memcmp(p, srcmac, 6), 0)<<  "Failed ethernet source MAC check";
+    EXPECT_EQ_UINT32(memcmp(p, dstmac, 6), 0)<<  "Failed ethernet dest MAC check";
+    // IP
+    EXPECT_EQ_UINT32(ip->getSourceIp(), l_pkt_test_s_ip)<<  "Failed IP src check";
+    EXPECT_EQ_UINT32(ip->getDestIp(), l_pkt_test_d_ip)<<  "Failed IP dst check";
+    EXPECT_EQ_UINT32(ip->getProtocol(), proto)<<  "Failed IP protocol check";
+    EXPECT_EQ_UINT32(ip->isChecksumOK()?0:1, 0)<<  "Failed IP checksum check";
+    EXPECT_EQ_UINT32(ip->getTimeToLive(), 0xff)<<  "Failed IP ttl check";
+    EXPECT_EQ_UINT32(ip->getTotalLength(), 48)<<  "Failed IP total length check";
+    
+    // payload
+    h=(latency_header *)(p+42);
+    EXPECT_EQ_UINT32(h->magic, LATENCY_MAGIC)<<  "Failed latency magic check";
+
+    if (proto == 0x84)
+        return true;
+    // ICMP
+    ICMPHeader *icmp = (ICMPHeader *)(p + 34);
+    EXPECT_EQ_UINT32(icmp->isCheckSumOk(28)?0:1, 0)<<  "Failed ICMP checksum verification";
+    EXPECT_EQ_UINT32(icmp->getSeqNum(), icmp_seq)<< "Failed ICMP sequence number check";
+    EXPECT_EQ_UINT32(icmp->getType(), icmp_type)<<  "Failed ICMP type check";
+    EXPECT_EQ_UINT32(icmp->getCode(), 0)<<  "Failed ICMP code check";
+    EXPECT_EQ_UINT32(icmp->getId(), 0xaabb)<<  "Failed ICMP ID check";
+
+    return true;
+}
+
+bool
+test_latency_pkt_rcv(rte_mbuf_t *m, uint8_t l_pkt_mode, uint8_t port_num, uint16_t num_pkt, bool exp_pkt_ok
+                     , uint16_t exp_tx_seq, uint16_t exp_rx_seq) {
+    CLatencyPktMode *c_l_pkt_mode;
+    CCPortLatency port;
+    CLatencyPktInfo info;
+    CLatencyManager mgr;
+    CRx_check_header *rxc;
+    CGlobalInfo::m_options.m_l_pkt_mode = l_pkt_mode;
+
+    c_l_pkt_mode = latency_pkt_mod_create(l_pkt_mode);
+    mgr.c_l_pkt_mode = c_l_pkt_mode;
+    info.Create(c_l_pkt_mode);
+    port.Create(&mgr, 0, info.get_payload_offset(), info.get_l4_offset(), info.get_pkt_size(), 0);
+    bool pkt_ok = port.check_packet(m, rxc);
+
+    while (num_pkt > 0) {
+        num_pkt--;
+        if (port.can_send_packet(port_num)) {
+            port.update_packet(m, 0);
         }
-        rte_pktmbuf_free(m);
     }
-    port0.DumpCounters(stdout);
-    EXPECT_EQ_UINT32(port0.m_pkt_ok, (uint32_t)100)<< "pass";
 
-    port0.Delete();
-    l.Delete();
+    EXPECT_EQ_UINT32(pkt_ok ?0:1, exp_pkt_ok?0:1)<<  "Failed packet OK check";
+    EXPECT_EQ_UINT32(port.get_icmp_tx_seq(), exp_tx_seq)<<  "Failed tx check";
+    EXPECT_EQ_UINT32(port.get_icmp_rx_seq(), exp_rx_seq)<<  "Failed rx check";
+    // if packet is bad, check_packet raise the error counter
+    EXPECT_EQ_UINT32(port.m_unsup_prot, exp_pkt_ok?0:1)<<  "Failed unsupported packets count";
+
+    return true;
 }
 
 
-TEST_F(basic, latency3) {
-    CLatencyPktInfo l;
-    CCPortLatency port0;
-    l.Create();
-    port0.Create(0,0,l.get_payload_offset(),l.get_pkt_size(),0);
+// Testing latency packet generation
+TEST_F(basic, latency1) {
+    uint8_t *p;
+    rte_mbuf_t * m;
+    uint16_t pkt_size;
 
+    // SCTP packet
+    m = create_latency_test_pkt(0, pkt_size, 0, 1);
+    p=rte_pktmbuf_mtod(m, uint8_t*);
+    verify_latency_pkt(p, 0x84, 0, 0);
+    rte_pktmbuf_free(m);
 
-    uint8_t  mac[]={0,0,0,1,0,0};
-    (void)mac;
+    m = create_latency_test_pkt(L_PKT_SUBMODE_NO_REPLY, pkt_size, 1, 2);
+    p=rte_pktmbuf_mtod(m, uint8_t*);
+    verify_latency_pkt(p, 0x01, 2, 8);
+    rte_pktmbuf_free(m);
 
+    // ICMP reply mode client side
+    m = create_latency_test_pkt(L_PKT_SUBMODE_REPLY, pkt_size, 0, 3);
+    p = rte_pktmbuf_mtod(m, uint8_t*);
+    verify_latency_pkt(p, 0x01, 3, 8);
+    rte_pktmbuf_free(m);
 
-    mac[0]=0;
-    mac[1]=0;
-    mac[2]=0;
-    mac[3]=1;
-    mac[4]=0;
-    mac[5]=0;
+    // ICMP reply mode server side
+    m = create_latency_test_pkt(L_PKT_SUBMODE_REPLY, pkt_size, 1, 4);
+    p=rte_pktmbuf_mtod(m, uint8_t*);
+    verify_latency_pkt(p, 0x01, 4, 0);
+    rte_pktmbuf_free(m);
 
+    m = create_latency_test_pkt(L_PKT_SUBMODE_0_SEQ, pkt_size, 1, 5);
+    p=rte_pktmbuf_mtod(m, uint8_t*);
+    verify_latency_pkt(p, 0x01, 0, 8);
+    rte_pktmbuf_free(m);
+}
 
-    l.set_ip(0x01000000,0x02000000,0x01000000);
+// Testing latency packet receive path
+TEST_F(basic, latency2) {
+    rte_mbuf_t *m;
+    uint16_t pkt_size;
+    uint8_t port_id = 0;
+    uint8_t pkt_num = 1;
+    uint8_t l_pkt_mode;
 
-    uint8_t *p;
-    rte_mbuf_t * m=l.generate_pkt(0);
-    port0.update_packet(m);
-    p=rte_pktmbuf_mtod(m, uint8_t*);
-    memset(p,0,l.get_pkt_size());
+    l_pkt_mode = 0;
+    m = create_latency_test_pkt(l_pkt_mode, pkt_size, port_id, pkt_num);
+    test_latency_pkt_rcv(m, l_pkt_mode, 0, 2, true, 1, 0);
 
-    CRx_check_header *  rx_p;
-    bool res=port0.check_packet(m,rx_p);
-    EXPECT_EQ_UINT32((uint32_t)res?0:1, (uint32_t)1)<< "pass";
-    if (!res ) {
-        printf(" OK \n");
-    }
-    rte_pktmbuf_free(m);
-    EXPECT_EQ_UINT32(port0.m_unsup_prot, (uint32_t)1)<< "pass";
+    l_pkt_mode = L_PKT_SUBMODE_NO_REPLY;
+    m = create_latency_test_pkt(l_pkt_mode, pkt_size, port_id, pkt_num);
+    test_latency_pkt_rcv(m, l_pkt_mode, 1, 2, true, 3, 1);
 
-    port0.Delete();
-    l.Delete();
-}
+    // reply mode server
+    l_pkt_mode = L_PKT_SUBMODE_REPLY;
+    m = create_latency_test_pkt(l_pkt_mode, pkt_size, port_id, pkt_num);
+    test_latency_pkt_rcv(m, l_pkt_mode, 1, 2, true, 2, 1);
 
+    // reply mode client
+    l_pkt_mode = L_PKT_SUBMODE_REPLY;
+    m = create_latency_test_pkt(l_pkt_mode, pkt_size, port_id, pkt_num);
+    test_latency_pkt_rcv(m, l_pkt_mode, 0, 2, true, 3, 1);
 
+    l_pkt_mode = L_PKT_SUBMODE_0_SEQ;
+    m = create_latency_test_pkt(l_pkt_mode, pkt_size, port_id, pkt_num);
+    test_latency_pkt_rcv(m, l_pkt_mode, 0, 2, true, 0, 0);
 
+    // bad packet
+    m = create_latency_test_pkt(l_pkt_mode, pkt_size, port_id, pkt_num);
+    EthernetHeader *eth = (EthernetHeader *)rte_pktmbuf_mtod(m, uint8_t*);
+    eth->setNextProtocol(0x5);
+    test_latency_pkt_rcv(m, l_pkt_mode, 0, 2, false, 1, 0);
+}
 
 class CDummyLatencyHWBase : public CPortLatencyHWBase {
 public:
@@ -835,21 +872,8 @@ public:
     uint8_t      m_port_id;
 };
 
-
-
-TEST_F(basic, latency4) {
-
-    uint8_t  mac[]={0,0,0,1,0,0};
-    (void)mac;
-    
-    mac[0]=0;
-    mac[1]=0;
-    mac[2]=0;
-    mac[3]=1;
-    mac[4]=0;
-    mac[5]=0;
-
-    
+// Testing latency statistics functions
+TEST_F(basic, latency3) {
     CLatencyManager mg;
     CLatencyManagerCfg  cfg;
     CDummyLatencyHWBase dports[MAX_LATENCY_PORTS];
@@ -882,7 +906,6 @@ TEST_F(basic, latency4) {
     mg.Delete();
 }
 
-
 TEST_F(basic, hist1) {
 
     CTimeHistogram  hist1;
@@ -902,8 +925,6 @@ TEST_F(basic, hist1) {
     hist1.Delete();
 }
 
-
-
 TEST_F(basic, rtsp1) {
 
      CTestBasic t1;
diff --git a/src/bp_sim.cpp b/src/bp_sim.cpp
index a61fbb8f..1383518b 100755
--- a/src/bp_sim.cpp
+++ b/src/bp_sim.cpp
@@ -20,6 +20,7 @@ limitations under the License.
 */
 
 #include "bp_sim.h"
+#include "latency.h"
 #include "utl_json.h"
 #include "utl_yaml.h"
 #include "msg_manager.h"
@@ -1311,7 +1312,6 @@ bool CPacketIndication::ConvertPacketToIpv6InPlace(CCapPktRaw * pkt,
     return (true);
 }
 
-
 void CPacketIndication::ProcessPacket(CPacketParser *parser,
                                       CCapPktRaw * pkt){
     _ProcessPacket(parser,pkt);
@@ -1320,8 +1320,6 @@ void CPacketIndication::ProcessPacket(CPacketParser *parser,
     }
 }
 
-
-
 /* process packet */
 void CPacketIndication::_ProcessPacket(CPacketParser *parser,
                                       CCapPktRaw * pkt){
@@ -4719,900 +4717,10 @@ int CErfIF::send_node(CGenNode * node){
    return (0);
 }
 
-
 int CErfIF::flush_tx_queue(void){
     return (0);
 }
 
-
-
-const uint8_t sctp_pkt[]={ 
-
-    0x00,0x04,0x96,0x08,0xe0,0x40,
-    0x00,0x0e,0x2e,0x24,0x37,0x5f,
-    0x08,0x00,
-
-    0x45,0x02,0x00,0x30,
-    0x00,0x00,0x40,0x00,
-    0x40,0x84,0xbd,0x04,
-    0x9b,0xe6,0x18,0x9b, //sIP
-    0xcb,0xff,0xfc,0xc2, //DIP
-
-    0x80,0x44,//SPORT
-    0x00,0x50,//DPORT
-
-    0x00,0x00,0x00,0x00, //checksum 
-
-    0x11,0x22,0x33,0x44, // magic 
-    0x00,0x00,0x00,0x00, //64 bit counter
-    0x00,0x00,0x00,0x00,
-    0x00,0x01,0xa0,0x00, //seq
-    0x00,0x00,0x00,0x00,
-
-};
-
-// 20+8+20`
-
-void CLatencyPktInfo::Create(){
-    m_packet = new CCapPktRaw( sizeof(sctp_pkt) );
-    m_packet->pkt_cnt=0;
-    m_packet->time_sec=0;
-    m_packet->time_nsec=0;
-    memcpy(m_packet->raw,sctp_pkt,sizeof(sctp_pkt));
-    m_packet->pkt_len=sizeof(sctp_pkt);
-
-    m_pkt_indication.m_packet  =m_packet;
-
-    m_pkt_indication.m_ether = (EthernetHeader *)m_packet->raw;
-    m_pkt_indication.l3.m_ipv4=(IPHeader       *)(m_packet->raw+14);
-    m_pkt_indication.m_is_ipv6 = false;
-    m_pkt_indication.l4.m_udp=(UDPHeader *)m_packet->raw+14+20;
-    m_pkt_indication.m_payload=(uint8_t *)m_packet->raw+14+20+16;
-    m_pkt_indication.m_payload_len=0;
-    m_pkt_indication.m_packet_padding=4;
-
-
-    m_pkt_indication.m_ether_offset =0;
-    m_pkt_indication.m_ip_offset =14;
-    m_pkt_indication.m_udp_tcp_offset = 34;
-    m_pkt_indication.m_payload_offset = 34+8;
-
-    CPacketDescriptor * lpd=&m_pkt_indication.m_desc;
-    lpd->Clear();
-    lpd->SetInitSide(true);
-    lpd->SetSwapTuple(false);
-    lpd->SetIsValidPkt(true);
-    lpd->SetIsUdp(true);
-    lpd->SetIsLastPkt(true);
-    m_pkt_info.Create(&m_pkt_indication);
-
-    memset(&m_dummy_node,0,sizeof(m_dummy_node));
-
-    m_dummy_node.set_socket_id( CGlobalInfo::m_socket.port_to_socket(0) );
-
-    m_dummy_node.m_time =0.1;
-    m_dummy_node.m_pkt_info = &m_pkt_info;
-    m_dummy_node.m_dest_ip  = 0;
-    m_dummy_node.m_src_ip   = 0;
-    m_dummy_node.m_src_port =  0x11;
-    m_dummy_node.m_flow_id =0;
-    m_dummy_node.m_flags =CGenNode::NODE_FLAGS_LATENCY;
-    
-}
-
-
-rte_mbuf_t * CLatencyPktInfo::generate_pkt(int port_id,uint32_t extern_ip){
-
-    bool is_client_to_serever=(port_id%2==0)?true:false;
-
-    int dual_port_index=(port_id>>1);
-    uint32_t c=m_client_ip.v4;
-    uint32_t s=m_server_ip.v4;
-    if ( extern_ip ){
-        c=extern_ip;
-    }
-
-    if (!is_client_to_serever) {
-        /*swap */
-        uint32_t t=c;
-        c=s;
-        s=t;
-    }
-    uint32_t mask=dual_port_index*m_dual_port_mask;
-    if ( extern_ip==0 ){
-       c+=mask;
-    }
-    s+=mask;
-    m_dummy_node.m_src_ip  = c;
-    m_dummy_node.m_dest_ip   = s;
-
-    rte_mbuf_t * m=m_pkt_info.generate_new_mbuf(&m_dummy_node);
-    return (m);
-
-
-}
-
-
-void CLatencyPktInfo::set_ip(uint32_t src,
-                             uint32_t dst,
-                             uint32_t dual_port_mask){
-
-    m_client_ip.v4=src;
-    m_server_ip.v4=dst;
-    m_dual_port_mask=dual_port_mask;
-
-}
-
-
-void CLatencyPktInfo::Delete(){
-    m_pkt_info.Delete();
-    delete m_packet;
-}
-
-void CCPortLatency::reset(){
-    m_rx_seq    =m_tx_seq;
-    m_pad       = 0;
-
-    m_tx_pkt_err=0;
-    m_tx_pkt_ok =0;
-    m_pkt_ok=0;
-    m_rx_check=0;
-    m_no_magic=0;
-    m_unsup_prot=0;
-    m_no_id=0;
-    m_seq_error=0;
-    m_length_error=0;
-    m_no_ipv4_option=0;
-    m_hist.Reset();
-}
-
-
-static uint8_t nat_is_port_can_send(uint8_t port_id){
-    uint8_t client_index = (port_id %2);
-    return (client_index ==0 ?1:0);
-}
-
-
-bool CCPortLatency::Create(CLatencyManager * parent,
-                           uint8_t id,
-                           uint16_t offset,
-                           uint16_t pkt_size,
-                           CCPortLatency * rx_port){
-    m_parent    = parent;
-    m_id        = id;
-    m_tx_seq    =0x12345678;
-    m_offset    = offset;
-    m_pkt_size  = pkt_size;
-    m_rx_port   = rx_port;
-    m_nat_can_send = nat_is_port_can_send(m_id);
-    m_nat_learn    = m_nat_can_send;
-    m_nat_external_ip=0;
-
-    m_hist.Create();
-    reset();
-    return (true);
-}
-
-void CCPortLatency::Delete(){
-    m_hist.Delete();
-}
-
-void CCPortLatency::update_packet(rte_mbuf_t * m){
-    uint8_t *p=rte_pktmbuf_mtod(m, uint8_t*);
-    /* update mac addr dest/src 12 bytes */
-    memcpy(p,CGlobalInfo::m_options.get_dst_src_mac_addr(m_id),12);
-
-    latency_header * h=(latency_header *)(p+m_offset);
-    h->magic = LATENCY_MAGIC | m_id ;
-    h->time_stamp = os_get_hr_tick_64();
-    h->seq = m_tx_seq;
-    m_tx_seq++;
-}
-
-
-void CCPortLatency::DumpShortHeader(FILE *fd){
-    
-
-    fprintf(fd," if|   tx_ok , rx_ok  , rx   ,error,    average   ,   max         , Jitter ,  max window \n");
-	fprintf(fd,"   |         ,        , check,     , latency(usec),latency (usec) ,(usec)  ,             \n");
-    fprintf(fd," ---------------------------------------------------------------------------------------------------------------- \n");
-}
-
-
-
-std::string CCPortLatency::get_field(std::string name,float f){
-    char buff[200];
-    sprintf(buff,"\"%s-%d\":%.1f,",name.c_str(),m_id,f);
-    return (std::string(buff));
-}
-
-
-void CCPortLatency::dump_json_v2(std::string & json ){
-    char buff[200];
-    sprintf(buff,"\"port-%d\": {",m_id);
-    json+=std::string(buff);
-    m_hist.dump_json("hist",json);
-    dump_counters_json(json);
-    json+="},";
-}
-
-void CCPortLatency::dump_json(std::string & json ){
-    json += get_field("avg",m_hist.get_average_latency() );
-    json += get_field("max",m_hist.get_max_latency() );
-    json += get_field("c-max",m_hist.get_max_latency_last_update() );
-    json += get_field("error",(float)(m_unsup_prot+m_no_magic+m_no_id+m_seq_error+m_length_error) );
-    json += get_field("jitter",(float)get_jitter_usec() );
-}
-
-
-void CCPortLatency::DumpShort(FILE *fd){
-
-	m_hist.update();
-    fprintf(fd,"%8lu,%8lu,%10lu,%4lu,",                          
-                    m_tx_pkt_ok,
-                    m_pkt_ok,
-                    m_rx_check,
-                    m_unsup_prot+m_no_magic+m_no_id+m_seq_error+m_length_error+m_no_ipv4_option+m_tx_pkt_err
-            );
-
-    fprintf(fd,"   %8.0f  ,%8.0f,%8d ",
-                          m_hist.get_average_latency(),
-                          m_hist.get_max_latency(),
-                          get_jitter_usec()
-                        );
-	fprintf(fd,"     | ");
-	m_hist.DumpWinMax(fd);
-
-}
-
-#define DPL_J(f)  json+=add_json(#f,f);
-#define DPL_J_LAST(f)  json+=add_json(#f,f,true);
-
-void CCPortLatency::dump_counters_json(std::string & json ){
-
-    json+="\"stats\" : {";
-    DPL_J(m_tx_pkt_ok);
-    DPL_J(m_tx_pkt_err);
-    DPL_J(m_pkt_ok);
-    DPL_J(m_unsup_prot);
-    DPL_J(m_no_magic);
-    DPL_J(m_no_id);
-    DPL_J(m_seq_error);
-    DPL_J(m_length_error);
-    DPL_J(m_no_ipv4_option);
-    json+=add_json("m_jitter",get_jitter_usec());
-    /* must be last */
-    DPL_J_LAST(m_rx_check);
-    json+="}";
-
-
-}
-
-void CCPortLatency::DumpCounters(FILE *fd){
-    #define DP_A1(f) if (f) fprintf(fd," %-40s : %llu \n",#f, (unsigned long long)f)
-
-    fprintf(fd," counter  \n");
-    fprintf(fd," -----------\n");
-
-    DP_A1(m_tx_pkt_err);
-    DP_A1(m_tx_pkt_ok);
-    DP_A1(m_pkt_ok);
-    DP_A1(m_unsup_prot);
-    DP_A1(m_no_magic);
-    DP_A1(m_no_id);
-    DP_A1(m_seq_error);
-    DP_A1(m_length_error);
-    DP_A1(m_rx_check);
-    DP_A1(m_no_ipv4_option);
-
-
-    fprintf(fd," -----------\n");
-    m_hist.Dump(fd);
-    fprintf(fd," %-40s : %lu \n","jitter", (ulong)get_jitter_usec());
-}
-
-bool CCPortLatency::dump_packet(rte_mbuf_t * m){
-    fprintf(stdout," %f.03 dump packet ..\n",now_sec());
-	uint8_t *p=rte_pktmbuf_mtod(m, uint8_t*);
-	uint16_t pkt_size=rte_pktmbuf_pkt_len(m);
-    utl_DumpBuffer(stdout,p,pkt_size,0);
-    return (0);
-
-
-
-    if (pkt_size < ( sizeof(CRx_check_header)+14+20) ) {
-        assert(0);
-    }
-    CRx_check_header * lp=(CRx_check_header *)(p+pkt_size-sizeof(CRx_check_header));
-
-    lp->dump(stdout);
-
-
-	uint16_t vlan_offset=0;
-	if ( unlikely( CGlobalInfo::m_options.preview.get_vlan_mode_enable() ) ){
-		vlan_offset=4;
-	}
-
-    (void)vlan_offset;
-
-//	utl_DumpBuffer(stdout,p,pkt_size,0);
-	return (0);
-
-}
-
-bool CCPortLatency::check_rx_check(rte_mbuf_t * m){
-    m_rx_check++;
-    return (true);
-}
-
-bool CCPortLatency::do_learn(uint32_t external_ip){
-    m_nat_learn=true;
-    m_nat_can_send=true;
-    m_nat_external_ip=external_ip;
-    return (true);
-}
-
-bool CCPortLatency::check_packet(rte_mbuf_t * m,CRx_check_header * & rx_p){
-
-    CSimplePacketParser parser(m);
-    if ( !parser.Parse()  ){
-        m_unsup_prot++;  // Unsupported protocol
-        return (false);
-    }
-    
-    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;
-
-    if ( !parser.IsLatencyPkt() ){
-
-        #ifdef NAT_TRACE_
-        printf(" %.3f RX : got packet !!! \n",now_sec() );
-        #endif
-
-        /* ipv6+rx-check */
-        if ( parser.m_ipv6 ) {
-            /* if we have ipv6 packet */
-            if (parser.m_protocol == RX_CHECK_V6_OPT_TYPE) {
-                if ( get_is_rx_check_mode() ){
-                    m_rx_check++;
-                    rx_p=(CRx_check_header *)((uint8_t*)parser.m_ipv6  +IPv6Header::DefaultSize);
-                    return (true);
-                }
-
-            }
-            m_seq_error++;
-           return (false);
-        }
-
-        uint8_t opt_len = parser.m_ipv4->getOptionLen();
-        uint8_t *opt_ptr = parser.m_ipv4->getOption();
-        /* Process IP option header(s) */
-        while ( opt_len != 0 ) {
-            switch (*opt_ptr) {
-            case RX_CHECK_V4_OPT_TYPE:
-                    /* rx-check option header */
-                    if ( ( !get_is_rx_check_mode() ) ||
-                        (opt_len < RX_CHECK_LEN) ) {
-                        m_seq_error++;
-                        return (false);
-                    }
-                    m_rx_check++;
-                    rx_p=(CRx_check_header *)opt_ptr;
-                    opt_len -= RX_CHECK_LEN;
-                    opt_ptr += RX_CHECK_LEN;
-                    break;
-            case CNatOption::noIPV4_OPTION:
-                    /* NAT learn option header */
-                    CNatOption *lp;
-                    if ( ( !CGlobalInfo::is_learn_mode() ) ||
-                         (opt_len < CNatOption::noOPTION_LEN) ) {
-                        m_seq_error++;
-                        return (false);
-                    }
-                    lp = (CNatOption *)opt_ptr;
-                    if ( !lp->is_valid_ipv4_magic() ) {
-                        m_no_ipv4_option++;
-                        return (false);
-                    }
-                    m_parent->get_nat_manager()->handle_packet_ipv4(lp,parser.m_ipv4);
-                    opt_len -= CNatOption::noOPTION_LEN;
-                    opt_ptr += CNatOption::noOPTION_LEN;
-                    break;
-            default:
-                    m_seq_error++;
-                    return (false);
-            } // End of switch
-        } // End of while
-
-        return (true);
-    } // End of check for non-latency packet
-
-    if ( CGlobalInfo::is_learn_mode() && (m_nat_learn ==false) ) {
-        do_learn(parser.m_ipv4->getSourceIp());
-    }
-
-    if ( (pkt_size-vlan_offset) != m_pkt_size ) {
-        m_length_error++;
-        return (false);
-    }
-
-    latency_header * h=(latency_header *)(p+m_offset+vlan_offset);
-
-    if ( (h->magic & 0xffffff00) != LATENCY_MAGIC ){
-        m_no_magic++;
-        return (false);
-    }
-
-    if ( h->seq != m_rx_seq ){
-        m_seq_error++;
-        m_rx_seq =h->seq +1;
-        return (false);
-    }else{
-        m_rx_seq++;
-    }
-    m_pkt_ok++;
-    uint64_t d = (os_get_hr_tick_64() - h->time_stamp );
-    dsec_t ctime=ptime_convert_hr_dsec(d);
-    m_hist.Add(ctime);
-    m_jitter.calc(ctime);
-    return (true);
-}
-
-void CLatencyManager::Delete(){
-    m_pkt_gen.Delete();
-
-    if ( get_is_rx_check_mode() ) {
-        m_rx_check_manager.Delete();
-    }
-    if ( CGlobalInfo::is_learn_mode() ){
-        m_nat_check_manager.Delete();
-    }
-    m_cpu_cp_u.Delete();
-}
-
-/* 0->1
-   1->0 
-   2->3
-   3->2
-*/
-static uint8_t swap_port(uint8_t port_id){
-    uint8_t offset= ((port_id>>1)<<1);
-    uint8_t client_index = (port_id %2);
-    return (offset + (client_index ^ 1));
-}
-
-
-
-bool CLatencyManager::Create(CLatencyManagerCfg * cfg){
-    m_max_ports=cfg->m_max_ports;
-    assert (m_max_ports<=MAX_LATENCY_PORTS);
-    assert ((m_max_ports%2)==0);
-    m_port_mask =0xffffffff;
-    m_do_stop =false;
-    m_is_active =false;
-    m_pkt_gen.Create();
-    int i;
-    for (i=0; i<m_max_ports; i++) {
-        CLatencyManagerPerPort * lp=&m_ports[i];
-        CCPortLatency * lpo=&m_ports[swap_port(i)].m_port;
-
-        lp->m_io=cfg->m_ports[i];
-        lp->m_port.Create(this,
-                          i,
-                          m_pkt_gen.get_payload_offset(),
-                          m_pkt_gen.get_pkt_size(),lpo );
-    }
-    m_cps= cfg->m_cps;
-    m_d_time =ptime_convert_dsec_hr((1.0/m_cps));
-    m_delta_sec =(1.0/m_cps);
-
-        
-    if ( get_is_rx_check_mode() ) {
-        assert(m_rx_check_manager.Create());
-        m_rx_check_manager.m_cur_time= now_sec();
-     }
-
-
-    m_pkt_gen.set_ip(cfg->m_client_ip.v4,cfg->m_server_ip.v4,cfg->m_dual_port_mask);
-    m_cpu_cp_u.Create(&m_cpu_dp_u);
-    if ( CGlobalInfo::is_learn_mode() ){
-        m_nat_check_manager.Create();
-    }
-    return (true);
-}
-
-
-void  CLatencyManager::send_pkt_all_ports(){
-    m_start_time = os_get_hr_tick_64();
-    int i;
-    for (i=0; i<m_max_ports; i++) {
-        if ( m_port_mask & (1<<i)  ){
-            CLatencyManagerPerPort * lp=&m_ports[i];
-            if (lp->m_port.can_send_packet() ){
-                rte_mbuf_t * m=m_pkt_gen.generate_pkt(i,lp->m_port.external_nat_ip());
-                lp->m_port.update_packet(m);
-                if ( lp->m_io->tx(m) == 0 ){
-                    lp->m_port.m_tx_pkt_ok++;
-                }else{
-                    lp->m_port.m_tx_pkt_err++;
-                }
-
-            }
-        }
-    }
-}
-
-
-void  CLatencyManager::wait_for_rx_dump(){
-	rte_mbuf_t * rx_pkts[64];
-	int i;
-	while ( true  ) {
-		rte_pause();
-		rte_pause();
-		rte_pause();
-		for (i=0; i<m_max_ports; i++) {
-			CLatencyManagerPerPort * lp=&m_ports[i];
-			rte_mbuf_t * m;
-			uint16_t cnt_p = lp->m_io->rx_burst(rx_pkts, 64);
-			if (cnt_p) {
-				int j;
-				for (j=0; j<cnt_p; j++) {
-					m=rx_pkts[j] ;
-					lp->m_port.dump_packet( m);
-					rte_pktmbuf_free(m);
-				}
-			} /*cnt_p*/
-		}/* for*/
-	}
-}
-
-
-void CLatencyManager::handle_rx_pkt(CLatencyManagerPerPort * lp,
-                                    rte_mbuf_t * m){
-    CRx_check_header *rxc;
-    lp->m_port.check_packet(m,rxc);
-    if ( unlikely(rxc!=NULL) ){
-        m_rx_check_manager.handle_packet(rxc);
-    }
-    rte_pktmbuf_free(m);
-}
-
-void CLatencyManager::handle_latecy_pkt_msg(uint8_t thread_id,
-                                            CGenNodeLatencyPktInfo * msg){
-
-    assert(msg->m_latency_offset==0xdead);
-
-    uint8_t rx_port_index=(thread_id<<1)+(msg->m_dir&1);
-    assert( rx_port_index <m_max_ports ) ;
-    CLatencyManagerPerPort * lp=&m_ports[rx_port_index];
-    handle_rx_pkt(lp,(rte_mbuf_t *)msg->m_pkt);
-}
-
-
-void  CLatencyManager::run_rx_queue_msgs(uint8_t thread_id,
-                                         CNodeRing * r){
-
-    while ( true ) {
-        CGenNode * node;
-        if ( r->Dequeue(node)!=0 ){
-            break;
-        }
-        assert(node);
-
-        CGenNodeMsgBase * msg=(CGenNodeMsgBase *)node;
-
-        uint8_t   msg_type =  msg->m_msg_type;
-        switch (msg_type ) {
-        case CGenNodeMsgBase::LATENCY_PKT:
-            handle_latecy_pkt_msg(thread_id,(CGenNodeLatencyPktInfo *) msg);
-            break;
-        default:
-            printf("ERROR latency-thread message type is not valid %d \n",msg_type);
-            assert(0);
-        }
-
-        CGlobalInfo::free_node(node);
-    }
-}
-
-void  CLatencyManager::try_rx_queues(){
-
-    CMessagingManager * rx_dp = CMsgIns::Ins()->getRxDp();
-    uint8_t threads=CMsgIns::Ins()->get_num_threads();
-    int ti;
-    for (ti=0; ti<(int)threads; ti++) {
-        CNodeRing * r = rx_dp->getRingDpToCp(ti);
-        if ( !r->isEmpty() ){
-            run_rx_queue_msgs((uint8_t)ti,r);
-        }
-    }
-}
-
-
-void  CLatencyManager::try_rx(){
-    rte_mbuf_t * rx_pkts[64];
-    int i;
-    for (i=0; i<m_max_ports; i++) {
-        CLatencyManagerPerPort * lp=&m_ports[i];
-        rte_mbuf_t * m;
-        m_cpu_dp_u.start_work();
-        /* try to read 64 packets clean up the queue */
-        uint16_t cnt_p = lp->m_io->rx_burst(rx_pkts, 64);
-        if (cnt_p) {
-            int j;
-            for (j=0; j<cnt_p; j++) {
-                m=rx_pkts[j] ;
-                handle_rx_pkt(lp,m);
-            }
-            /* commit only if there was work to do ! */
-            m_cpu_dp_u.commit();
-          }/* if work */
-      }// all ports
-}
-
-
-void  CLatencyManager::reset(){
-
-    int i;
-    for (i=0; i<m_max_ports; i++) {
-        CLatencyManagerPerPort * lp=&m_ports[i];
-        lp->m_port.reset();
-    }
-
-}
-
-void  CLatencyManager::start(int iter){
-    m_do_stop =false;
-    m_is_active =false;
-    int cnt=0;
-
-    double n_time;
-    CGenNode * node = new CGenNode();
-    node->m_type = CGenNode::FLOW_SYNC;   /* general stuff */
-    node->m_time = now_sec()+0.007;
-    m_p_queue.push(node);
-
-    node = new CGenNode();
-    node->m_type = CGenNode::FLOW_PKT; /* latency */
-    node->m_time = now_sec(); /* 1/cps rate */
-    m_p_queue.push(node);
-    bool do_try_rx_queue =CGlobalInfo::m_options.preview.get_vm_one_queue_enable()?true:false;
-
-
-    while (  !m_p_queue.empty() ) {
-        node = m_p_queue.top();
-        n_time = node->m_time;
-
-        /* wait for event */
-        while ( true ) {
-            double dt = now_sec() - n_time ;
-            if (dt> (0.0)) {
-                break;
-            }
-            if (do_try_rx_queue){
-                try_rx_queues();
-            }
-            try_rx();
-            rte_pause();
-        }
-
-        switch (node->m_type) {
-        case CGenNode::FLOW_SYNC:
-            if ( CGlobalInfo::is_learn_mode() ) {
-                m_nat_check_manager.handle_aging();
-            }
-
-            m_p_queue.pop();
-            node->m_time += SYNC_TIME_OUT;
-            m_p_queue.push(node);
-
-            break;
-        case CGenNode::FLOW_PKT:
-            m_cpu_dp_u.start_work();
-            send_pkt_all_ports();
-            m_p_queue.pop();
-            node->m_time += m_delta_sec;
-            m_p_queue.push(node);
-            m_cpu_dp_u.commit();
-            break;
-        }
-
-        /* this will be called every sync which is 1msec */
-        if ( m_do_stop ) {
-             break;
-        }
-        if ( iter>0   ){
-            if ( ( cnt>iter) ){
-                printf("stop due iter %d\n",iter);
-                break;
-            }
-        } 
-        cnt++;
-    }
-
-    /* free all nodes in the queue */
-    while (!m_p_queue.empty()) {
-        node = m_p_queue.top();
-        m_p_queue.pop();
-        delete node;
-    }
-
-    printf(" latency daemon has stopped\n");
-    if ( get_is_rx_check_mode() ) {
-        m_rx_check_manager.tw_drain();
-    }
-
-}
-
-void  CLatencyManager::stop(){
-    m_do_stop =true;
-}
-
-bool  CLatencyManager::is_active(){
-    return (m_is_active);
-}
-
-
-double CLatencyManager::get_max_latency(){
-    double l=0.0;
-    int i;
-    for (i=0; i<m_max_ports; i++) {
-        CLatencyManagerPerPort * lp=&m_ports[i];
-        if ( l <lp->m_port.m_hist.get_max_latency() ){
-            l=lp->m_port.m_hist.get_max_latency();
-        }
-    }
-    return (l);
-}
-
-double CLatencyManager::get_avr_latency(){
-    double l=0.0;
-    int i;
-    for (i=0; i<m_max_ports; i++) {
-        CLatencyManagerPerPort * lp=&m_ports[i];
-        if ( l <lp->m_port.m_hist.get_average_latency() ){
-            l=lp->m_port.m_hist.get_average_latency();
-        }
-    }
-    return (l);
-}
-
-uint64_t CLatencyManager::get_total_pkt(){
-    int i;
-    uint64_t t=0;
-    for (i=0; i<m_max_ports; i++) {
-        CLatencyManagerPerPort * lp=&m_ports[i];
-        t+=lp->m_port.m_tx_pkt_ok ;
-    }
-    return  t;
-}
-
-uint64_t CLatencyManager::get_total_bytes(){
-    int i;
-    uint64_t t=0;
-    for (i=0; i<m_max_ports; i++) {
-        CLatencyManagerPerPort * lp=&m_ports[i];
-        t+=lp->m_port.m_tx_pkt_ok* (m_pkt_gen.get_pkt_size()+4);
-    }
-    return  t;
-
-}
-
-
-bool   CLatencyManager::is_any_error(){
-    int i;
-    for (i=0; i<m_max_ports; i++) {
-        CLatencyManagerPerPort * lp=&m_ports[i];
-        if ( lp->m_port.is_any_err() ){
-            return (true);
-        }
-    }
-    return  (false);
-}
-
-
-void CLatencyManager::dump_json(std::string & json ){
-    json="{\"name\":\"trex-latecny\",\"type\":0,\"data\":{";
-    int i;
-    for (i=0; i<m_max_ports; i++) {
-        CLatencyManagerPerPort * lp=&m_ports[i];
-        lp->m_port.dump_json(json);
-    }
-
-    json+="\"unknown\":0}}"  ;
-
-}
-
-void CLatencyManager::dump_json_v2(std::string & json ){
-    json="{\"name\":\"trex-latecny-v2\",\"type\":0,\"data\":{";
-    json+=add_json("cpu_util",m_cpu_cp_u.GetVal());
-
-    int i;
-    for (i=0; i<m_max_ports; i++) {
-        CLatencyManagerPerPort * lp=&m_ports[i];
-        lp->m_port.dump_json_v2(json);
-    }
-
-    json+="\"unknown\":0}}"  ;
-
-}
-
-void CLatencyManager::DumpRxCheck(FILE *fd){
-    if ( get_is_rx_check_mode() ) {
-        fprintf(fd," rx checker : \n");
-        m_rx_check_manager.DumpShort(fd);
-        m_rx_check_manager.Dump(fd);
-    }
-}
-
-void CLatencyManager::DumpShortRxCheck(FILE *fd){
-    if ( get_is_rx_check_mode() ) {
-        m_rx_check_manager.DumpShort(fd);
-    }
-}
-
-void CLatencyManager::rx_check_dump_json(std::string & json){
-    if ( get_is_rx_check_mode() ) {
-        m_rx_check_manager.dump_json(json );
-    }
-}
-
-void CLatencyManager::update(){
-    m_cpu_cp_u.Update() ;
-}
-
-void CLatencyManager::DumpShort(FILE *fd){
-    int i;
-    fprintf(fd," Cpu Utilization : %2.1f %%  \n",m_cpu_cp_u.GetVal());
-    CCPortLatency::DumpShortHeader(fd);
-    for (i=0; i<m_max_ports; i++) {
-        fprintf(fd," %d | ",i);
-        CLatencyManagerPerPort * lp=&m_ports[i];
-        lp->m_port.DumpShort(fd);
-        fprintf(fd,"\n");
-    }
-
-
-}
-
-void CLatencyManager::Dump(FILE *fd){
-    int i;
-    fprintf(fd," cpu : %2.1f  %% \n",m_cpu_cp_u.GetVal());
-    for (i=0; i<m_max_ports; i++) {
-        fprintf(fd," port   %d \n",i);
-        fprintf(fd," -----------------\n");
-        CLatencyManagerPerPort * lp=&m_ports[i];
-        lp->m_port.DumpCounters(fd);
-    }
-}
-
-void CLatencyManager::DumpRxCheckVerification(FILE *fd,
-                                              uint64_t total_tx_rx_check){
-    if ( !get_is_rx_check_mode() ) {
-        fprintf(fd," rx_checker is disabled  \n");
-        return;
-    }
-    fprintf(fd," rx_check Tx : %llu \n", (unsigned long long)total_tx_rx_check);
-    fprintf(fd," rx_check Rx : %llu \n", (unsigned long long)m_rx_check_manager.getTotalRx() );
-    fprintf(fd," rx_check verification :" );
-    if (m_rx_check_manager.getTotalRx() == total_tx_rx_check) {
-        fprintf(fd," OK \n" );
-    }else{
-        fprintf(fd," FAIL \n" );
-    }
-}
-
-
-
 void CTcpSeq::update(uint8_t *p, CFlowPktInfo *pkt_info, int16_t s_size){
     TCPHeader *tcp= (TCPHeader *)(p+pkt_info->m_pkt_indication.getFastTcpOffset());
     uint32_t seqnum, acknum;
@@ -6865,6 +5973,7 @@ bool CSimplePacketParser::Parse(){
     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;
@@ -6880,6 +5989,7 @@ bool CSimplePacketParser::Parse(){
         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;
@@ -6929,7 +6039,3 @@ void CGenNodeBase::free_base(){
 
 }
 
-
-
-
-
diff --git a/src/bp_sim.h b/src/bp_sim.h
index 0da7fb99..3d81c759 100755
--- a/src/bp_sim.h
+++ b/src/bp_sim.h
@@ -37,6 +37,7 @@ limitations under the License.
 #include <string>
 #include <common/Network/Packet/TcpHeader.h>
 #include <common/Network/Packet/UdpHeader.h>
+#include <common/Network/Packet/IcmpHeader.h>
 #include <common/Network/Packet/IPHeader.h>
 #include <common/Network/Packet/IPv6Header.h>
 #include <common/Network/Packet/EthernetHeader.h>
@@ -749,6 +750,7 @@ public:
         prefix="";
         m_mac_splitter=0;
         m_run_mode = RUN_MODE_INVALID;
+        m_l_pkt_mode = 0;
     }
 
 
@@ -771,7 +773,7 @@ public:
     uint16_t        m_io_mode; //0,1,2 0 disable, 1- normal , 2 - short 
     uint16_t        m_run_flags;
     uint8_t         m_mac_splitter;
-    uint8_t         m_pad;
+    uint8_t         m_l_pkt_mode;
     trex_run_mode_e    m_run_mode;
 
 
@@ -828,6 +830,9 @@ public:
         return (  (m_run_flags &RUN_FLAGS_RXCHECK_CONST_TS)?true:false );
     }
 
+    inline uint8_t get_l_pkt_mode(){
+        return (m_l_pkt_mode);
+    }
     void dump(FILE *fd);
 };
 
@@ -1237,7 +1242,8 @@ static inline int get_is_rx_check_mode(){
 }
 
 static inline bool get_is_rx_filter_enable(){
-    return ( ( get_is_rx_check_mode() || CGlobalInfo::is_learn_mode()) ?true:false );
+    uint32_t latency_rate=CGlobalInfo::m_options.m_latency_rate;
+    return ( ( get_is_rx_check_mode() || CGlobalInfo::is_learn_mode() || latency_rate != 0) ?true:false );
 }
 static inline uint16_t get_rx_check_hops() {
     return (CGlobalInfo::m_options.m_rx_check_hops);
@@ -2061,11 +2067,8 @@ inline bool CFlowKey::operator ==(const CFlowKey& rhs) const{
 #define IS_VALID_S 1
 #define IS_VALID_E 1
 
-#define IS_TCP_S 2
-#define IS_TCP_E 2
-
-#define IS_UDP_S 3
-#define IS_UDP_E 3
+#define PROTO_S 3
+#define PROTO_E 2
 
 #define IS_INIT_SIDE 4
 
@@ -2235,19 +2238,27 @@ public:
     }
 
     inline void SetIsTcp(bool is_valid){
-        btSetMaskBit32(m_flags,IS_TCP_S,IS_TCP_E,is_valid?1:0);
+        btSetMaskBit32(m_flags,PROTO_S,PROTO_E,is_valid?1:0);
     }
 
     inline bool IsTcp(){
-        return (btGetMaskBit32(m_flags,IS_TCP_S,IS_TCP_E) ? true:false);
+        return ((btGetMaskBit32(m_flags,PROTO_S,PROTO_E) == 1) ? true:false);
     }
 
     inline void SetIsUdp(bool is_valid){
-        btSetMaskBit32(m_flags,IS_UDP_S,IS_UDP_E,is_valid?1:0);
+        btSetMaskBit32(m_flags,PROTO_S,PROTO_E,is_valid?2:0);
     }
 
     inline bool IsUdp(){
-        return (btGetMaskBit32(m_flags,IS_UDP_S,IS_UDP_E) ? true:false);
+        return ((btGetMaskBit32(m_flags,PROTO_S,PROTO_E) == 2) ? true:false);
+    }
+
+    inline void SetIsIcmp(bool is_valid){
+        btSetMaskBit32(m_flags,PROTO_S,PROTO_E,is_valid?3:0);
+    }
+
+    inline bool IsIcmp(){
+        return ((btGetMaskBit32(m_flags,PROTO_S,PROTO_E) == 3) ? true:false);
     }
 
     inline void SetId(uint16_t _id){
@@ -2379,6 +2390,7 @@ public:
     union {                             
         TCPHeader * m_tcp;
         UDPHeader * m_udp;
+        ICMPHeader * m_icmp;      
     } l4;
     uint8_t *       m_payload;
     uint16_t        m_payload_len;
@@ -2902,7 +2914,11 @@ inline void CFlowPktInfo::update_pkt_info(char *p,
                 m_udp->setDestPort(src_port);
             }
         }else{
-            BP_ASSERT(0);
+#ifdef _DEBUG
+            if (!m_pkt_indication.m_desc.IsIcmp()) {
+               BP_ASSERT(0);
+            }
+#endif
         }
     }
 }
@@ -3733,312 +3749,6 @@ inline void CFlowGeneratorRecPerThread::generate_flow(CNodeGenerator * gen,
                                node);
 }
 
-
-
-class CLatencyPktInfo {
-public:
-    void Create();
-    void Delete();
-    void set_ip(uint32_t src,
-                uint32_t dst,
-                uint32_t dual_port_mask);
-    rte_mbuf_t * generate_pkt(int port_id,uint32_t extern_ip=0);
-
-    CGenNode   *    getNode(){
-        return (&m_dummy_node);
-    }
-
-    uint16_t get_payload_offset(void){
-        return ( m_pkt_indication.getFastPayloadOffset());
-    }
-
-    uint16_t get_pkt_size(void){
-        return ( m_packet->pkt_len );
-    }
-
-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;
-
-    uint8_t get_id(){
-        return( magic & 0xff);
-    }
-};
-
-
-class CSimplePacketParser {
-public:
-
-    CSimplePacketParser(rte_mbuf_t * m){
-        m_m=m;
-    }
-
-    bool Parse();
-    uint8_t getTTl();
-    uint16_t getPktSize();
-
-
-
-    inline bool IsLatencyPkt(){
-        return ( (m_protocol ==0x84 )?true:false );
-    }
-
-
-public:
-    IPHeader *      m_ipv4;
-    IPv6Header *    m_ipv6;
-    uint8_t         m_protocol;
-    uint16_t        m_vlan_offset;
-    uint16_t        m_option_offset;
-private: 
-    rte_mbuf_t *    m_m ;
-};
-
-
-
-class CLatencyManager ;
-// per port 
-class CCPortLatency {
-public:
-    bool Create(CLatencyManager * parent,
-                uint8_t id,
-                uint16_t offset,
-                uint16_t pkt_size,
-                CCPortLatency * rx_port
-                );
-    void Delete();
-    void reset();
-    bool can_send_packet(){
-        if ( !CGlobalInfo::is_learn_mode() ) {
-            return(true);
-        }
-        return ( m_nat_can_send );
-    }
-    uint32_t external_nat_ip(){
-        return (m_nat_external_ip);
-    }
-
-    void update_packet(rte_mbuf_t * m);
-
-    bool do_learn(uint32_t external_ip);
-
-    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);
-
-    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 ) && 
-
-              ((m_unsup_prot+
-                m_no_magic+
-                m_no_id+
-                m_seq_error+
-                m_length_error+m_no_ipv4_option+m_tx_pkt_err)==0) ) {
-            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_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_offset;
-
-     uint16_t m_pkt_size;
-     uint16_t pad1[3];
-
-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;
-     uint64_t m_no_id;
-     uint64_t m_seq_error;
-     uint64_t m_rx_check;
-     uint64_t m_no_ipv4_option;
-
-
-     uint64_t m_length_error;
-     CTimeHistogram  m_hist; /* all window */
-     CJitter         m_jitter; 
-};
-
-
-class CPortLatencyHWBase {
-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, 
-                               uint16_t nb_pkts){
-        return(0);
-    }
-};
-
-
-class CLatencyManagerCfg {
-public:
-    CLatencyManagerCfg (){
-        m_max_ports=0;
-        m_cps=0.0;
-        m_client_ip.v4=0x10000000;
-        m_server_ip.v4=0x20000000;
-        m_dual_port_mask=0x01000000;
-    }
-    uint32_t             m_max_ports;
-    double               m_cps;// CPS
-    CPortLatencyHWBase * m_ports[MAX_LATENCY_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 CLatencyManager {
-public:
-    bool Create(CLatencyManagerCfg * cfg);
-    void Delete();
-
-public:
-    void  reset();
-    void  start(int iter);
-    void  stop();
-    bool  is_active();
-
-    void set_ip(uint32_t client_ip,
-                uint32_t server_ip,
-                uint32_t mask_dual_port){
-        m_pkt_gen.set_ip(client_ip,server_ip,mask_dual_port);
-    }
-
-public:
-    void Dump(FILE *fd); // dump all
-    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 rx_check_dump_json(std::string & json);
-    uint16_t get_latency_header_offset(){
-        return ( m_pkt_gen.get_payload_offset() );
-    }
-    void update();
-    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){
-        m_port_mask=mask;
-    }
-
-    double get_max_latency(void);
-    double get_avr_latency(void);
-    bool   is_any_error();
-    uint64_t get_total_pkt();
-    uint64_t get_total_bytes();
-    CNatRxManager * get_nat_manager(){
-        return ( &m_nat_check_manager );
-    }
-
-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);
-
-
-private:
-    /* messages handlers */
-    void handle_latecy_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[MAX_LATENCY_PORTS];
-     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 
-     uint32_t                m_port_mask;
-     uint32_t                m_max_ports;
-	 RxCheckManager 		 m_rx_check_manager;
-     CNatRxManager           m_nat_check_manager;
-     CCpuUtlDp               m_cpu_dp_u;
-     CCpuUtlCp               m_cpu_cp_u;
-
-     volatile bool           m_do_stop __rte_cache_aligned ;
-
-};
-
-
 inline bool CGenNode::is_responder_pkt(){
     return ( m_pkt_info->m_pkt_indication.m_desc.IsInitSide() ?false:true );
 }
diff --git a/src/common/Network/Packet/IcmpHeader.h b/src/common/Network/Packet/IcmpHeader.h
new file mode 100644
index 00000000..99d89329
--- /dev/null
+++ b/src/common/Network/Packet/IcmpHeader.h
@@ -0,0 +1,89 @@
+/*
+Copyright (c) 2015-2015 Cisco Systems, Inc.
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+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.
+*/
+
+#ifndef _ICMP_HEADER_H_
+#define _ICMP_HEADER_H_
+
+#include "PacketHeaderBase.h"
+#include "IPHeader.h"
+
+class ICMPHeader
+{
+
+public:
+    ICMPHeader()
+	{
+		setCode(0);
+		setType(0);
+		setSeqNum(0xDEAD);
+		setId(0xBEEF);
+		setChecksum(0);
+	}
+
+    ICMPHeader(uint8_t  argType,
+	       uint8_t  argCode,
+	       uint16_t argId,
+	       uint16_t argSeqNum)
+	{
+		setType(argType);
+		setCode(argCode);
+		setId(argId);
+		setSeqNum(argSeqNum);
+	}
+
+
+    inline  void setCode(uint8_t data);
+    inline  uint8_t getCode();
+
+    inline  void setType(uint8_t data);
+    inline  uint8_t getType();
+
+    inline  void setSeqNum(uint16_t data);
+    inline  uint16_t getSeqNum();
+
+    inline  void setId(uint16_t data);
+    inline  uint16_t getId();
+
+    inline  void setChecksum(uint16_t data);
+    inline  uint16_t getChecksum();
+
+    inline void updateCheckSum(uint16_t len);
+    inline bool isCheckSumOk(uint16_t len);
+    inline uint16_t calcCheckSum(uint16_t len);
+
+
+////////////////////////////////////////////////////////////////////////////////////////
+// Common Header Interface
+////////////////////////////////////////////////////////////////////////////////////////
+
+public: 
+    inline	uint8_t*  getPointer          (){return (uint8_t*)this;}
+    inline	uint32_t  getSize             (){return 8;}
+
+    void    dump                (FILE*  fd);
+
+private:
+    uint8_t   myType;
+    uint8_t   myCode;
+    uint16_t  myChecksum;
+    uint16_t  myId;
+    uint16_t  mySeqNum;
+};
+
+
+#include "IcmpHeader.inl"
+
+#endif 
diff --git a/src/common/Network/Packet/IcmpHeader.inl b/src/common/Network/Packet/IcmpHeader.inl
new file mode 100644
index 00000000..0e6806f8
--- /dev/null
+++ b/src/common/Network/Packet/IcmpHeader.inl
@@ -0,0 +1,87 @@
+/*
+Copyright (c) 2015-2015 Cisco Systems, Inc.
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+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.
+*/
+
+inline void ICMPHeader::setCode(uint8_t argCode)
+{
+    myCode = argCode;
+}
+
+inline uint8_t ICMPHeader::getCode()
+{
+    return myCode;
+}
+
+inline void   ICMPHeader::setType(uint8_t argType)
+{
+    myType = argType;
+}
+
+inline uint8_t ICMPHeader::getType()
+{
+    return myType;
+}
+
+inline void    ICMPHeader::setSeqNum(uint16_t argSeqNum)
+{
+    mySeqNum = PKT_NTOHS(argSeqNum);
+}
+
+inline uint16_t ICMPHeader::getSeqNum()
+{
+    return PKT_NTOHS(mySeqNum);
+}
+
+inline void    ICMPHeader::setId(uint16_t argId)
+{
+    myId = PKT_NTOHS(argId);
+}
+
+inline uint16_t ICMPHeader::getId()
+{
+    return PKT_NTOHS(myId);
+}
+
+inline void ICMPHeader::setChecksum(uint16_t argNewChecksum)
+{
+    myChecksum = PKT_NTOHS(argNewChecksum);
+}
+
+inline uint16_t ICMPHeader::getChecksum()
+{
+    return PKT_NTOHS(myChecksum);
+}
+
+inline void ICMPHeader::updateCheckSum(uint16_t len)
+{
+	setChecksum(0);// must be here 
+
+	myChecksum =calcCheckSum(len);
+}
+
+inline bool ICMPHeader::isCheckSumOk(uint16_t len)
+{
+	uint16_t theChecksum= PKT_NTOHS(calcCheckSum(len));
+
+	return(theChecksum == 0);
+}
+
+// len is in bytes. Including ICMP header + data.
+inline uint16_t ICMPHeader::calcCheckSum(uint16_t len)
+{
+	uint16_t theChecksum = pkt_InetChecksum((uint8_t*)this, len);
+
+	return(theChecksum);
+}
diff --git a/src/common/bitMan.h b/src/common/bitMan.h
index ffa05598..8019b3f7 100755
--- a/src/common/bitMan.h
+++ b/src/common/bitMan.h
@@ -160,8 +160,8 @@ inline void btSetMaskBit32(unsigned int & a,
      btSetMaskBit<unsigned int>(a,startbit,stopbit,newVal);
 }
 
-/* start > stop             startbit = 10 , 
-                            stop    = 8 
+/* Notice: 
+   startbit should be bigger (or equal) than stopbit
 
 count like big E
 
diff --git a/src/latency.cpp b/src/latency.cpp
new file mode 100644
index 00000000..198ea2c7
--- /dev/null
+++ b/src/latency.cpp
@@ -0,0 +1,1017 @@
+/*
+ Hanoh Haim
+ Ido Barnea
+ Cisco Systems, Inc.
+*/
+
+/*
+Copyright (c) 2015-2015 Cisco Systems, Inc.
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+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 "latency.h"
+#include "bp_sim.h"
+#include "utl_json.h"
+#include <common/basic_utils.h> 
+
+const uint8_t sctp_pkt[]={ 
+
+    0x00,0x04,0x96,0x08,0xe0,0x40,
+    0x00,0x0e,0x2e,0x24,0x37,0x5f,
+    0x08,0x00,
+
+    0x45,0x02,0x00,0x30,
+    0x00,0x00,0x40,0x00,
+    0xff,0x84,0xbd,0x04,
+    0x9b,0xe6,0x18,0x9b, //sIP
+    0xcb,0xff,0xfc,0xc2, //DIP
+
+    0x80,0x44,//SPORT
+    0x00,0x50,//DPORT
+
+    0x00,0x00,0x00,0x00, //checksum 
+
+    0x11,0x22,0x33,0x44, // magic 
+    0x00,0x00,0x00,0x00, //64 bit counter
+    0x00,0x00,0x00,0x00,
+    0x00,0x01,0xa0,0x00, //seq
+    0x00,0x00,0x00,0x00,
+
+};
+
+const uint8_t icmp_pkt[]={
+    0x00,0x04,0x96,0x08,0xe0,0x40,
+    0x00,0x0e,0x2e,0x24,0x37,0x5f,
+    0x08,0x00,
+
+    0x45,0x02,0x00,0x30,
+    0x00,0x00,0x40,0x00,
+    0xff,0x01,0xbd,0x04,
+    0x9b,0xe6,0x18,0x9b, //SIP
+    0xcb,0xff,0xfc,0xc2, //DIP
+
+    0x08, 0x00, 
+    0x01, 0x02,  //checksum
+    0xaa, 0xbb,  // id
+    0x00, 0x00,  // Sequence number
+
+    0x11,0x22,0x33,0x44, // magic 
+    0x00,0x00,0x00,0x00, //64 bit counter
+    0x00,0x00,0x00,0x00,
+    0x00,0x01,0xa0,0x00, //seq
+    0x00,0x00,0x00,0x00,
+
+}; 
+
+
+void CLatencyPktInfo::Create(class CLatencyPktMode *m_l_pkt_info){
+    uint8_t pkt_size = m_l_pkt_info->getPacketLen();
+
+    m_packet = new CCapPktRaw( pkt_size);
+    m_packet->pkt_cnt=0;
+    m_packet->time_sec=0;
+    m_packet->time_nsec=0;
+    memcpy(m_packet->raw, m_l_pkt_info->getPacketData(), pkt_size);
+    m_packet->pkt_len=pkt_size;
+
+    m_pkt_indication.m_packet  =m_packet;
+
+    m_pkt_indication.m_ether = (EthernetHeader *)m_packet->raw;
+    m_pkt_indication.l3.m_ipv4=(IPHeader       *)(m_packet->raw+14);
+    m_pkt_indication.m_is_ipv6 = false;
+    m_pkt_indication.l4.m_icmp=(ICMPHeader *)m_packet->raw+14+20;
+    m_pkt_indication.m_payload=(uint8_t *)m_packet->raw+14+20+16;
+    m_pkt_indication.m_payload_len=0;
+    m_pkt_indication.m_packet_padding=4;
+
+
+    m_pkt_indication.m_ether_offset =0;
+    m_pkt_indication.m_ip_offset =14;
+    m_pkt_indication.m_udp_tcp_offset = 34;
+    m_pkt_indication.m_payload_offset = 34+8;
+
+    CPacketDescriptor * lpd=&m_pkt_indication.m_desc;
+    lpd->Clear();
+    lpd->SetInitSide(true);
+    lpd->SetSwapTuple(false);
+    lpd->SetIsValidPkt(true);
+    lpd->SetIsIcmp(true);
+    lpd->SetIsLastPkt(true);
+    m_pkt_info.Create(&m_pkt_indication);
+
+    memset(&m_dummy_node,0,sizeof(m_dummy_node));
+
+    m_dummy_node.set_socket_id( CGlobalInfo::m_socket.port_to_socket(0) );
+
+    m_dummy_node.m_time =0.1;
+    m_dummy_node.m_pkt_info = &m_pkt_info;
+    m_dummy_node.m_dest_ip  = 0;
+    m_dummy_node.m_src_ip   = 0;
+    m_dummy_node.m_src_port =  0x11;
+    m_dummy_node.m_flow_id =0;
+    m_dummy_node.m_flags =CGenNode::NODE_FLAGS_LATENCY;
+    
+}
+
+rte_mbuf_t * CLatencyPktInfo::generate_pkt(int port_id,uint32_t extern_ip){
+    bool is_client_to_server=(port_id%2==0)?true:false;
+
+    int dual_port_index=(port_id>>1);
+    uint32_t c=m_client_ip.v4;
+    uint32_t s=m_server_ip.v4;
+    if ( extern_ip ){
+        c=extern_ip;
+    }
+
+    if (!is_client_to_server) {
+        /*swap */
+        uint32_t t=c;
+        c=s;
+        s=t;
+    }
+    uint32_t mask=dual_port_index*m_dual_port_mask;
+    if ( extern_ip==0 ){
+       c+=mask;
+    }
+    s+=mask;
+    m_dummy_node.m_src_ip  = c;
+    m_dummy_node.m_dest_ip   = s;
+
+    rte_mbuf_t * m=m_pkt_info.generate_new_mbuf(&m_dummy_node);
+    return (m);
+}
+
+void CLatencyPktInfo::set_ip(uint32_t src,
+                             uint32_t dst,
+                             uint32_t dual_port_mask){
+    m_client_ip.v4=src;
+    m_server_ip.v4=dst;
+    m_dual_port_mask=dual_port_mask;
+}
+
+void CLatencyPktInfo::Delete(){
+    m_pkt_info.Delete();
+    delete m_packet;
+}
+
+void CCPortLatency::reset(){
+    m_rx_seq    =m_tx_seq;
+    m_pad       = 0;
+    m_tx_pkt_err=0;
+    m_tx_pkt_ok =0;
+    m_pkt_ok=0;
+    m_rx_check=0;
+    m_no_magic=0;
+    m_unsup_prot=0;
+    m_no_id=0;
+    m_seq_error=0;
+    m_length_error=0;
+    m_no_ipv4_option=0;
+    m_hist.Reset();
+}
+
+static uint8_t nat_is_port_can_send(uint8_t port_id){
+    uint8_t client_index = (port_id %2);
+    return (client_index ==0 ?1:0);
+}
+
+bool CCPortLatency::Create(CLatencyManager * parent,
+                           uint8_t id,
+                           uint16_t payload_offset,
+                           uint16_t l4_offset,
+                           uint16_t pkt_size,
+                           CCPortLatency * rx_port){
+    m_parent    = parent;
+    m_id        = id;
+    m_tx_seq    =0x12345678;
+    m_icmp_tx_seq = 1;
+    m_icmp_rx_seq = 0;
+    m_l4_offset = l4_offset;
+    m_payload_offset    = payload_offset;
+    m_pkt_size  = pkt_size;
+    m_rx_port   = rx_port;
+    m_nat_can_send = nat_is_port_can_send(m_id);
+    m_nat_learn    = m_nat_can_send;
+    m_nat_external_ip=0;
+
+    m_hist.Create();
+    reset();
+    return (true);
+}
+
+void CCPortLatency::Delete(){
+    m_hist.Delete();
+}
+
+void CCPortLatency::update_packet(rte_mbuf_t * m, int port_id){
+    uint8_t *p=rte_pktmbuf_mtod(m, uint8_t*);
+    bool is_client_to_server=(port_id%2==0)?true:false;
+
+    /* update mac addr dest/src 12 bytes */
+    memcpy(p,CGlobalInfo::m_options.get_dst_src_mac_addr(m_id),12);
+
+    latency_header * h=(latency_header *)(p+m_payload_offset);
+    h->magic = LATENCY_MAGIC | m_id ;
+    h->time_stamp = os_get_hr_tick_64();
+    h->seq = m_tx_seq;
+    m_tx_seq++;
+
+    CLatencyPktMode *c_l_pkt_mode = m_parent->c_l_pkt_mode;
+    c_l_pkt_mode->update_pkt(p + m_l4_offset, is_client_to_server, m_pkt_size - m_l4_offset, &m_icmp_tx_seq);    
+}
+
+
+void CCPortLatency::DumpShortHeader(FILE *fd){
+    
+
+    fprintf(fd," if|   tx_ok , rx_ok  , rx   ,error,    average   ,   max         , Jitter ,  max window \n");
+	fprintf(fd,"   |         ,        , check,     , latency(usec),latency (usec) ,(usec)  ,             \n");
+    fprintf(fd," ---------------------------------------------------------------------------------------------------------------- \n");
+}
+
+
+
+std::string CCPortLatency::get_field(std::string name,float f){
+    char buff[200];
+    sprintf(buff,"\"%s-%d\":%.1f,",name.c_str(),m_id,f);
+    return (std::string(buff));
+}
+
+
+void CCPortLatency::dump_json_v2(std::string & json ){
+    char buff[200];
+    sprintf(buff,"\"port-%d\": {",m_id);
+    json+=std::string(buff);
+    m_hist.dump_json("hist",json);
+    dump_counters_json(json);
+    json+="},";
+}
+
+void CCPortLatency::dump_json(std::string & json ){
+    json += get_field("avg",m_hist.get_average_latency() );
+    json += get_field("max",m_hist.get_max_latency() );
+    json += get_field("c-max",m_hist.get_max_latency_last_update() );
+    json += get_field("error",(float)(m_unsup_prot+m_no_magic+m_no_id+m_seq_error+m_length_error) );
+    json += get_field("jitter",(float)get_jitter_usec() );
+}
+
+
+void CCPortLatency::DumpShort(FILE *fd){
+
+	m_hist.update();
+    fprintf(fd,"%8lu,%8lu,%10lu,%4lu,",                          
+                    m_tx_pkt_ok,
+                    m_pkt_ok,
+                    m_rx_check,
+                    m_unsup_prot+m_no_magic+m_no_id+m_seq_error+m_length_error+m_no_ipv4_option+m_tx_pkt_err
+            );
+
+    fprintf(fd,"   %8.0f  ,%8.0f,%8d ",
+                          m_hist.get_average_latency(),
+                          m_hist.get_max_latency(),
+                          get_jitter_usec()
+                        );
+	fprintf(fd,"     | ");
+	m_hist.DumpWinMax(fd);
+
+}
+
+#define DPL_J(f)  json+=add_json(#f,f);
+#define DPL_J_LAST(f)  json+=add_json(#f,f,true);
+
+void CCPortLatency::dump_counters_json(std::string & json ){
+
+    json+="\"stats\" : {";
+    DPL_J(m_tx_pkt_ok);
+    DPL_J(m_tx_pkt_err);
+    DPL_J(m_pkt_ok);
+    DPL_J(m_unsup_prot);
+    DPL_J(m_no_magic);
+    DPL_J(m_no_id);
+    DPL_J(m_seq_error);
+    DPL_J(m_length_error);
+    DPL_J(m_no_ipv4_option);
+    json+=add_json("m_jitter",get_jitter_usec());
+    /* must be last */
+    DPL_J_LAST(m_rx_check);
+    json+="}";
+
+
+}
+
+void CCPortLatency::DumpCounters(FILE *fd){
+    #define DP_A1(f) if (f) fprintf(fd," %-40s : %llu \n",#f, (unsigned long long)f)
+
+    fprintf(fd," counter  \n");
+    fprintf(fd," -----------\n");
+
+    DP_A1(m_tx_pkt_err);
+    DP_A1(m_tx_pkt_ok);
+    DP_A1(m_pkt_ok);
+    DP_A1(m_unsup_prot);
+    DP_A1(m_no_magic);
+    DP_A1(m_no_id);
+    DP_A1(m_seq_error);
+    DP_A1(m_length_error);
+    DP_A1(m_rx_check);
+    DP_A1(m_no_ipv4_option);
+
+
+    fprintf(fd," -----------\n");
+    m_hist.Dump(fd);
+    fprintf(fd," %-40s : %lu \n","jitter", (ulong)get_jitter_usec());
+}
+
+bool CCPortLatency::dump_packet(rte_mbuf_t * m){
+    fprintf(stdout," %f.03 dump packet ..\n",now_sec());
+	uint8_t *p=rte_pktmbuf_mtod(m, uint8_t*);
+	uint16_t pkt_size=rte_pktmbuf_pkt_len(m);
+    utl_DumpBuffer(stdout,p,pkt_size,0);
+    return (0);
+
+
+
+    if (pkt_size < ( sizeof(CRx_check_header)+14+20) ) {
+        assert(0);
+    }
+    CRx_check_header * lp=(CRx_check_header *)(p+pkt_size-sizeof(CRx_check_header));
+
+    lp->dump(stdout);
+
+
+	uint16_t vlan_offset=0;
+	if ( unlikely( CGlobalInfo::m_options.preview.get_vlan_mode_enable() ) ){
+		vlan_offset=4;
+	}
+
+    (void)vlan_offset;
+
+//	utl_DumpBuffer(stdout,p,pkt_size,0);
+	return (0);
+
+}
+
+bool CCPortLatency::check_rx_check(rte_mbuf_t * m) {
+    m_rx_check++;
+    return (true);
+}
+
+bool CCPortLatency::do_learn(uint32_t external_ip) {
+    m_nat_learn=true;
+    m_nat_can_send=true;
+    m_nat_external_ip=external_ip;
+    return (true);
+}
+
+bool CCPortLatency::check_packet(rte_mbuf_t * m,CRx_check_header * & rx_p) {
+    CSimplePacketParser parser(m);
+    if ( !parser.Parse()  ) {
+        m_unsup_prot++;  // Unsupported protocol
+        return (false);
+    }    
+    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());
+
+    if ( ! is_lateancy_pkt) {
+
+#ifdef NAT_TRACE_
+        printf(" %.3f RX : got packet !!! \n",now_sec() );
+#endif
+
+        /* ipv6+rx-check */
+        if ( parser.m_ipv6 ) {
+            /* if we have ipv6 packet */
+            if (parser.m_protocol == RX_CHECK_V6_OPT_TYPE) {
+                if ( get_is_rx_check_mode() ){
+                    m_rx_check++;
+                    rx_p=(CRx_check_header *)((uint8_t*)parser.m_ipv6  +IPv6Header::DefaultSize);
+                    return (true);
+                }
+
+            }
+            m_seq_error++;
+           return (false);
+        }
+
+        uint8_t opt_len = parser.m_ipv4->getOptionLen();
+        uint8_t *opt_ptr = parser.m_ipv4->getOption();
+        /* Process IP option header(s) */
+        while ( opt_len != 0 ) {
+            switch (*opt_ptr) {
+            case RX_CHECK_V4_OPT_TYPE:
+                    /* rx-check option header */
+                    if ( ( !get_is_rx_check_mode() ) ||
+                        (opt_len < RX_CHECK_LEN) ) {
+                        m_seq_error++;
+                        return (false);
+                    }
+                    m_rx_check++;
+                    rx_p=(CRx_check_header *)opt_ptr;
+                    opt_len -= RX_CHECK_LEN;
+                    opt_ptr += RX_CHECK_LEN;
+                    break;
+            case CNatOption::noIPV4_OPTION:
+                    /* NAT learn option header */
+                    CNatOption *lp;
+                    if ( ( !CGlobalInfo::is_learn_mode() ) ||
+                         (opt_len < CNatOption::noOPTION_LEN) ) {
+                        m_seq_error++;
+                        return (false);
+                    }
+                    lp = (CNatOption *)opt_ptr;
+                    if ( !lp->is_valid_ipv4_magic() ) {
+                        m_no_ipv4_option++;
+                        return (false);
+                    }
+                    m_parent->get_nat_manager()->handle_packet_ipv4(lp,parser.m_ipv4);
+                    opt_len -= CNatOption::noOPTION_LEN;
+                    opt_ptr += CNatOption::noOPTION_LEN;
+                    break;
+            default:
+                    m_seq_error++;
+                    return (false);
+            } // End of switch
+        } // End of while
+
+        return (true);
+    } // End of check for non-latency packet
+    if ( CGlobalInfo::is_learn_mode() && (m_nat_learn ==false) ) {
+        do_learn(parser.m_ipv4->getSourceIp());
+    }
+
+    if ( (pkt_size-vlan_offset) != m_pkt_size ) {
+        m_length_error++;
+        return (false);
+    }
+    c_l_pkt_mode->update_recv(p + m_l4_offset + vlan_offset, &m_icmp_rx_seq, &m_icmp_tx_seq);
+#ifdef LATENCY_DEBUG
+    c_l_pkt_mode->rcv_debug_print(p + m_l4_offset + vlan_offset);
+#endif
+    latency_header * h=(latency_header *)(p+m_payload_offset + vlan_offset);
+    if ( h->seq != m_rx_seq ){
+        m_seq_error++;
+        m_rx_seq =h->seq +1;
+        return (false);
+    }else{
+        m_rx_seq++;
+    }
+    m_pkt_ok++;
+    uint64_t d = (os_get_hr_tick_64() - h->time_stamp );
+    dsec_t ctime=ptime_convert_hr_dsec(d);
+    m_hist.Add(ctime);
+    m_jitter.calc(ctime);
+    return (true);
+}
+
+void CLatencyManager::Delete(){
+    m_pkt_gen.Delete();
+
+    if ( get_is_rx_check_mode() ) {
+        m_rx_check_manager.Delete();
+    }
+    if ( CGlobalInfo::is_learn_mode() ){
+        m_nat_check_manager.Delete();
+    }
+    m_cpu_cp_u.Delete();
+}
+
+/* 0->1
+   1->0 
+   2->3
+   3->2
+*/
+static uint8_t swap_port(uint8_t port_id){
+    uint8_t offset= ((port_id>>1)<<1);
+    uint8_t client_index = (port_id %2);
+    return (offset + (client_index ^ 1));
+}
+
+
+
+bool CLatencyManager::Create(CLatencyManagerCfg * cfg){
+    switch (CGlobalInfo::m_options.get_l_pkt_mode()) {
+    default:
+    case 0:
+        c_l_pkt_mode = (CLatencyPktModeSCTP *) new CLatencyPktModeSCTP(CGlobalInfo::m_options.get_l_pkt_mode());
+        break;
+    case 1:
+    case 2:
+    case 3:
+        c_l_pkt_mode =  (CLatencyPktModeICMP *) new CLatencyPktModeICMP(CGlobalInfo::m_options.get_l_pkt_mode());
+        break;
+    }
+    
+    m_max_ports=cfg->m_max_ports;
+    assert (m_max_ports<=MAX_LATENCY_PORTS);
+    assert ((m_max_ports%2)==0);
+    m_port_mask =0xffffffff;
+    m_do_stop =false;
+    m_is_active =false;
+    m_pkt_gen.Create(c_l_pkt_mode);
+    int i;
+    for (i=0; i<m_max_ports; i++) {
+        CLatencyManagerPerPort * lp=&m_ports[i];
+        CCPortLatency * lpo=&m_ports[swap_port(i)].m_port;
+
+        lp->m_io=cfg->m_ports[i];
+        lp->m_port.Create(this,
+                          i,
+                          m_pkt_gen.get_payload_offset(),
+                          m_pkt_gen.get_l4_offset(),
+                          m_pkt_gen.get_pkt_size(),lpo );
+    }
+    m_cps= cfg->m_cps;
+    m_d_time =ptime_convert_dsec_hr((1.0/m_cps));
+    m_delta_sec =(1.0/m_cps);
+
+        
+    if ( get_is_rx_check_mode() ) {
+        assert(m_rx_check_manager.Create());
+        m_rx_check_manager.m_cur_time= now_sec();
+     }
+
+
+    m_pkt_gen.set_ip(cfg->m_client_ip.v4,cfg->m_server_ip.v4,cfg->m_dual_port_mask);
+    m_cpu_cp_u.Create(&m_cpu_dp_u);
+    if ( CGlobalInfo::is_learn_mode() ){
+        m_nat_check_manager.Create();
+    }
+    return (true);
+}
+
+
+void  CLatencyManager::send_pkt_all_ports(){
+    m_start_time = os_get_hr_tick_64();
+    int i;
+    for (i=0; i<m_max_ports; i++) {
+        if ( m_port_mask & (1<<i)  ){
+            CLatencyManagerPerPort * lp=&m_ports[i];
+            if (lp->m_port.can_send_packet(i%2) ){
+                rte_mbuf_t * m=m_pkt_gen.generate_pkt(i,lp->m_port.external_nat_ip());
+                lp->m_port.update_packet(m, i);
+
+#ifdef LATENCY_DEBUG
+                uint8_t *p = rte_pktmbuf_mtod(m, uint8_t*);
+                c_l_pkt_mode->send_debug_print(p + 34);
+#endif
+                if ( lp->m_io->tx(m) == 0 ){
+                    lp->m_port.m_tx_pkt_ok++;
+                }else{
+                    lp->m_port.m_tx_pkt_err++;
+                }
+
+            }
+        }
+    }
+}
+
+
+void  CLatencyManager::wait_for_rx_dump(){
+	rte_mbuf_t * rx_pkts[64];
+	int i;
+	while ( true  ) {
+		rte_pause();
+		rte_pause();
+		rte_pause();
+		for (i=0; i<m_max_ports; i++) {
+			CLatencyManagerPerPort * lp=&m_ports[i];
+			rte_mbuf_t * m;
+			uint16_t cnt_p = lp->m_io->rx_burst(rx_pkts, 64);
+			if (cnt_p) {
+				int j;
+				for (j=0; j<cnt_p; j++) {
+					m=rx_pkts[j] ;
+					lp->m_port.dump_packet( m);
+					rte_pktmbuf_free(m);
+				}
+			} /*cnt_p*/
+		}/* for*/
+	}
+}
+
+
+void CLatencyManager::handle_rx_pkt(CLatencyManagerPerPort * lp,
+                                    rte_mbuf_t * m){
+    CRx_check_header *rxc;
+    lp->m_port.check_packet(m,rxc);
+    if ( unlikely(rxc!=NULL) ){
+        m_rx_check_manager.handle_packet(rxc);
+    }
+    rte_pktmbuf_free(m);
+}
+
+void CLatencyManager::handle_latency_pkt_msg(uint8_t thread_id,
+                                            CGenNodeLatencyPktInfo * msg){
+
+    assert(msg->m_latency_offset==0xdead);
+
+    uint8_t rx_port_index=(thread_id<<1)+(msg->m_dir&1);
+    assert( rx_port_index <m_max_ports ) ;
+    CLatencyManagerPerPort * lp=&m_ports[rx_port_index];
+    handle_rx_pkt(lp,(rte_mbuf_t *)msg->m_pkt);
+}
+
+
+void  CLatencyManager::run_rx_queue_msgs(uint8_t thread_id,
+                                         CNodeRing * r){
+
+    while ( true ) {
+        CGenNode * node;
+        if ( r->Dequeue(node)!=0 ){
+            break;
+        }
+        assert(node);
+
+        CGenNodeMsgBase * msg=(CGenNodeMsgBase *)node;
+
+        uint8_t   msg_type =  msg->m_msg_type;
+        switch (msg_type ) {
+        case CGenNodeMsgBase::LATENCY_PKT:
+            handle_latency_pkt_msg(thread_id,(CGenNodeLatencyPktInfo *) msg);
+            break;
+        default:
+            printf("ERROR latency-thread message type is not valid %d \n",msg_type);
+            assert(0);
+        }
+
+        CGlobalInfo::free_node(node);
+    }
+}
+
+void  CLatencyManager::try_rx_queues(){
+
+    CMessagingManager * rx_dp = CMsgIns::Ins()->getRxDp();
+    uint8_t threads=CMsgIns::Ins()->get_num_threads();
+    int ti;
+    for (ti=0; ti<(int)threads; ti++) {
+        CNodeRing * r = rx_dp->getRingDpToCp(ti);
+        if ( !r->isEmpty() ){
+            run_rx_queue_msgs((uint8_t)ti,r);
+        }
+    }
+}
+
+
+void  CLatencyManager::try_rx(){
+    rte_mbuf_t * rx_pkts[64];
+    int i;
+    for (i=0; i<m_max_ports; i++) {
+        CLatencyManagerPerPort * lp=&m_ports[i];
+        rte_mbuf_t * m;
+        m_cpu_dp_u.start_work();
+        /* try to read 64 packets clean up the queue */
+        uint16_t cnt_p = lp->m_io->rx_burst(rx_pkts, 64);
+        if (cnt_p) {
+            int j;
+            for (j=0; j<cnt_p; j++) {
+                m=rx_pkts[j] ;
+                handle_rx_pkt(lp,m);
+            }
+            /* commit only if there was work to do ! */
+            m_cpu_dp_u.commit();
+          }/* if work */
+      }// all ports
+}
+
+
+void  CLatencyManager::reset(){
+
+    int i;
+    for (i=0; i<m_max_ports; i++) {
+        CLatencyManagerPerPort * lp=&m_ports[i];
+        lp->m_port.reset();
+    }
+
+}
+
+void  CLatencyManager::start(int iter){
+    m_do_stop =false;
+    m_is_active =false;
+    int cnt=0;
+
+    double n_time;
+    CGenNode * node = new CGenNode();
+    node->m_type = CGenNode::FLOW_SYNC;   /* general stuff */
+    node->m_time = now_sec()+0.007;
+    m_p_queue.push(node);
+
+    node = new CGenNode();
+    node->m_type = CGenNode::FLOW_PKT; /* latency */
+    node->m_time = now_sec(); /* 1/cps rate */
+    m_p_queue.push(node);
+    bool do_try_rx_queue =CGlobalInfo::m_options.preview.get_vm_one_queue_enable()?true:false;
+
+
+    while (  !m_p_queue.empty() ) {
+        node = m_p_queue.top();
+        n_time = node->m_time;
+
+        /* wait for event */
+        while ( true ) {
+            double dt = now_sec() - n_time ;
+            if (dt> (0.0)) {
+                break;
+            }
+            if (do_try_rx_queue){
+                try_rx_queues();
+            }
+            try_rx();
+            rte_pause();
+        }
+
+        switch (node->m_type) {
+        case CGenNode::FLOW_SYNC:
+            if ( CGlobalInfo::is_learn_mode() ) {
+                m_nat_check_manager.handle_aging();
+            }
+
+            m_p_queue.pop();
+            node->m_time += SYNC_TIME_OUT;
+            m_p_queue.push(node);
+
+            break;
+        case CGenNode::FLOW_PKT:
+            m_cpu_dp_u.start_work();
+            send_pkt_all_ports();
+            m_p_queue.pop();
+            node->m_time += m_delta_sec;
+            m_p_queue.push(node);
+            m_cpu_dp_u.commit();
+            break;
+        }
+
+        /* this will be called every sync which is 1msec */
+        if ( m_do_stop ) {
+             break;
+        }
+        if ( iter>0   ){
+            if ( ( cnt>iter) ){
+                printf("stop due iter %d\n",iter);
+                break;
+            }
+        } 
+        cnt++;
+    }
+
+    /* free all nodes in the queue */
+    while (!m_p_queue.empty()) {
+        node = m_p_queue.top();
+        m_p_queue.pop();
+        delete node;
+    }
+
+    printf(" latency daemon has stopped\n");
+    if ( get_is_rx_check_mode() ) {
+        m_rx_check_manager.tw_drain();
+    }
+
+}
+
+void  CLatencyManager::stop(){
+    m_do_stop =true;
+}
+
+bool  CLatencyManager::is_active(){
+    return (m_is_active);
+}
+
+
+double CLatencyManager::get_max_latency(){
+    double l=0.0;
+    int i;
+    for (i=0; i<m_max_ports; i++) {
+        CLatencyManagerPerPort * lp=&m_ports[i];
+        if ( l <lp->m_port.m_hist.get_max_latency() ){
+            l=lp->m_port.m_hist.get_max_latency();
+        }
+    }
+    return (l);
+}
+
+double CLatencyManager::get_avr_latency(){
+    double l=0.0;
+    int i;
+    for (i=0; i<m_max_ports; i++) {
+        CLatencyManagerPerPort * lp=&m_ports[i];
+        if ( l <lp->m_port.m_hist.get_average_latency() ){
+            l=lp->m_port.m_hist.get_average_latency();
+        }
+    }
+    return (l);
+}
+
+uint64_t CLatencyManager::get_total_pkt(){
+    int i;
+    uint64_t t=0;
+    for (i=0; i<m_max_ports; i++) {
+        CLatencyManagerPerPort * lp=&m_ports[i];
+        t+=lp->m_port.m_tx_pkt_ok ;
+    }
+    return  t;
+}
+
+uint64_t CLatencyManager::get_total_bytes(){
+    int i;
+    uint64_t t=0;
+    for (i=0; i<m_max_ports; i++) {
+        CLatencyManagerPerPort * lp=&m_ports[i];
+        t+=lp->m_port.m_tx_pkt_ok* (m_pkt_gen.get_pkt_size()+4);
+    }
+    return  t;
+
+}
+
+
+bool   CLatencyManager::is_any_error(){
+    int i;
+    for (i=0; i<m_max_ports; i++) {
+        CLatencyManagerPerPort * lp=&m_ports[i];
+        if ( lp->m_port.is_any_err() ){
+            return (true);
+        }
+    }
+    return  (false);
+}
+
+
+void CLatencyManager::dump_json(std::string & json ){
+    json="{\"name\":\"trex-latecny\",\"type\":0,\"data\":{";
+    int i;
+    for (i=0; i<m_max_ports; i++) {
+        CLatencyManagerPerPort * lp=&m_ports[i];
+        lp->m_port.dump_json(json);
+    }
+
+    json+="\"unknown\":0}}"  ;
+
+}
+
+void CLatencyManager::dump_json_v2(std::string & json ){
+    json="{\"name\":\"trex-latecny-v2\",\"type\":0,\"data\":{";
+    json+=add_json("cpu_util",m_cpu_cp_u.GetVal());
+
+    int i;
+    for (i=0; i<m_max_ports; i++) {
+        CLatencyManagerPerPort * lp=&m_ports[i];
+        lp->m_port.dump_json_v2(json);
+    }
+
+    json+="\"unknown\":0}}"  ;
+
+}
+
+void CLatencyManager::DumpRxCheck(FILE *fd){
+    if ( get_is_rx_check_mode() ) {
+        fprintf(fd," rx checker : \n");
+        m_rx_check_manager.DumpShort(fd);
+        m_rx_check_manager.Dump(fd);
+    }
+}
+
+void CLatencyManager::DumpShortRxCheck(FILE *fd){
+    if ( get_is_rx_check_mode() ) {
+        m_rx_check_manager.DumpShort(fd);
+    }
+}
+
+void CLatencyManager::rx_check_dump_json(std::string & json){
+    if ( get_is_rx_check_mode() ) {
+        m_rx_check_manager.dump_json(json );
+    }
+}
+
+void CLatencyManager::update(){
+    m_cpu_cp_u.Update() ;
+}
+
+void CLatencyManager::DumpShort(FILE *fd){
+    int i;
+    fprintf(fd," Cpu Utilization : %2.1f %%  \n",m_cpu_cp_u.GetVal());
+    CCPortLatency::DumpShortHeader(fd);
+    for (i=0; i<m_max_ports; i++) {
+        fprintf(fd," %d | ",i);
+        CLatencyManagerPerPort * lp=&m_ports[i];
+        lp->m_port.DumpShort(fd);
+        fprintf(fd,"\n");
+    }
+
+
+}
+
+void CLatencyManager::Dump(FILE *fd){
+    int i;
+    fprintf(fd," cpu : %2.1f  %% \n",m_cpu_cp_u.GetVal());
+    for (i=0; i<m_max_ports; i++) {
+        fprintf(fd," port   %d \n",i);
+        fprintf(fd," -----------------\n");
+        CLatencyManagerPerPort * lp=&m_ports[i];
+        lp->m_port.DumpCounters(fd);
+    }
+}
+
+void CLatencyManager::DumpRxCheckVerification(FILE *fd,
+                                              uint64_t total_tx_rx_check){
+    if ( !get_is_rx_check_mode() ) {
+        fprintf(fd," rx_checker is disabled  \n");
+        return;
+    }
+    fprintf(fd," rx_check Tx : %llu \n", (unsigned long long)total_tx_rx_check);
+    fprintf(fd," rx_check Rx : %llu \n", (unsigned long long)m_rx_check_manager.getTotalRx() );
+    fprintf(fd," rx_check verification :" );
+    if (m_rx_check_manager.getTotalRx() == total_tx_rx_check) {
+        fprintf(fd," OK \n" );
+    }else{
+        fprintf(fd," FAIL \n" );
+    }
+}
+
+uint8_t CLatencyPktModeICMP::getPacketLen() {return sizeof(icmp_pkt);}
+const uint8_t *CLatencyPktModeICMP::getPacketData() {return icmp_pkt;}
+void CLatencyPktModeICMP::rcv_debug_print(uint8_t *pkt) {
+    ICMPHeader *m_icmp = (ICMPHeader *)pkt;
+    printf ("received latency ICMP packet code:%d seq:%x\n"
+            , m_icmp->getType(), m_icmp->getSeqNum());
+};
+
+void CLatencyPktModeICMP::send_debug_print(uint8_t *pkt) {
+    ICMPHeader *m_icmp = (ICMPHeader *)pkt;
+    printf ("Sending latency ICMP packet code:%d seq:%d\n", m_icmp->getType(), m_icmp->getSeqNum());
+}
+
+void CLatencyPktModeICMP::update_pkt(uint8_t *pkt, bool is_client_to_server, uint16_t l4_len, uint16_t *tx_seq) {
+    ICMPHeader * m_icmp =(ICMPHeader *)(pkt);
+
+    if (m_submode == L_PKT_SUBMODE_0_SEQ) {
+        m_icmp->setSeqNum(0);
+    } else {
+        m_icmp->setSeqNum(*tx_seq);
+        (*tx_seq)++;
+    }
+
+    if ((!is_client_to_server) && (m_submode == L_PKT_SUBMODE_REPLY)) {
+      m_icmp->setType(0); // echo reply
+    } else {
+      m_icmp->setType(8); // echo request
+    }
+    // ICMP checksum is calculated on payload + ICMP header
+    m_icmp->updateCheckSum(l4_len);
+
+}
+
+bool CLatencyPktModeICMP::IsLatencyPkt(IPHeader *ip) {
+    if (!ip)
+        return false;
+    if (ip->getProtocol() != 0x1)
+        return false;
+    return true;
+};
+
+void CLatencyPktModeICMP::update_recv(uint8_t *pkt, uint16_t *r_seq, uint16_t *t_seq) {
+    ICMPHeader *m_icmp = (ICMPHeader *)(pkt);
+    *r_seq = m_icmp->getSeqNum();
+    // handle wrap around, so can_send_packet will allow us to send
+    if (*r_seq == 0)
+        *t_seq = 0;
+}
+
+
+uint8_t CLatencyPktModeSCTP::getPacketLen() {return sizeof(sctp_pkt);}
+const uint8_t *CLatencyPktModeSCTP::getPacketData() {return sctp_pkt;}
+void CLatencyPktModeSCTP::rcv_debug_print(uint8_t *pkt) {printf("Received latency SCTP packet\n");}
+void CLatencyPktModeSCTP::send_debug_print(uint8_t *pkt) {printf("Sending latency SCTP packet\n");
+    // utl_DumpBuffer(stdout,pkt-20,28,0);
+}
+void CLatencyPktModeSCTP::update_pkt(uint8_t *pkt, bool is_client_to_server, uint16_t l4_len, uint16_t *tx_seq) {}
+bool CLatencyPktModeSCTP::IsLatencyPkt(IPHeader *ip) {
+    if (!ip) {
+        return false;
+    }
+    if (ip->getProtocol() != 0x84) {
+        return false;
+    }
+    return true;
+};
+void CLatencyPktModeSCTP::update_recv(uint8_t *pkt, uint16_t *r_seq, uint16_t *t_seq) {}
diff --git a/src/latency.h b/src/latency.h
new file mode 100644
index 00000000..59481a59
--- /dev/null
+++ b/src/latency.h
@@ -0,0 +1,386 @@
+#ifndef LATENCY_H
+#define LATENCY_H
+/*
+ Hanoh Haim
+ Ido Barnea
+ Cisco Systems, Inc.
+*/
+
+/*
+Copyright (c) 2015-2015 Cisco Systems, Inc.
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+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>
+
+#define L_PKT_SUBMODE_NO_REPLY 1
+#define L_PKT_SUBMODE_REPLY 2
+#define L_PKT_SUBMODE_0_SEQ 3
+
+class CLatencyPktInfo {
+public:
+    void Create(class CLatencyPktMode *m_l_pkt_info);
+    void Delete();
+    void set_ip(uint32_t src,
+                uint32_t dst,
+                uint32_t dual_port_mask);
+    rte_mbuf_t * generate_pkt(int port_id,uint32_t extern_ip=0);
+
+    CGenNode   *    getNode(){
+        return (&m_dummy_node);
+    }
+
+    uint16_t get_payload_offset(void){
+        return ( m_pkt_indication.getFastPayloadOffset());
+    }
+
+    uint16_t get_l4_offset(void){
+        return ( m_pkt_indication.getFastTcpOffset());
+    }
+
+    uint16_t get_pkt_size(void){
+        return ( m_packet->pkt_len );
+    }
+
+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;
+
+    uint8_t get_id(){
+        return( magic & 0xff);
+    }
+};
+
+class CSimplePacketParser {
+public:
+
+    CSimplePacketParser(rte_mbuf_t * m){
+        m_m=m;
+    }
+
+    bool Parse();
+    uint8_t getTTl();
+    uint16_t getPktSize();
+
+    inline bool IsLatencyPkt(uint8_t *p) {
+        latency_header * h=(latency_header *)(p);
+        if ( (h->magic & 0xffffff00) != LATENCY_MAGIC ){
+            return (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 
+class CCPortLatency {
+public:
+    bool Create(CLatencyManager * parent,
+                uint8_t id,
+                uint16_t offset,
+                uint16_t l4_offset,
+                uint16_t pkt_size,
+                CCPortLatency * rx_port
+                );
+    void Delete();
+    void reset();
+    bool can_send_packet(int direction){
+        // in icmp_reply mode, can send response from server, only after we got the relevant request
+        // if we got request, we are sure to have NAT translation in place already.
+        if ((CGlobalInfo::m_options.m_l_pkt_mode == L_PKT_SUBMODE_REPLY) && (direction == 1)) {
+            if (m_icmp_tx_seq <= m_icmp_rx_seq)
+                return(true);
+            else
+                return(false);
+        }        
+
+        if ( !CGlobalInfo::is_learn_mode() ) {
+            return(true);
+        }
+        return ( m_nat_can_send );
+    }
+    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_rx_check(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 ) && 
+
+              ((m_unsup_prot+
+                m_no_magic+
+                m_no_id+
+                m_seq_error+
+                m_length_error+m_no_ipv4_option+m_tx_pkt_err)==0) ) {
+            return (false);
+        }
+        return (true);
+    }
+
+    uint16_t get_icmp_tx_seq() {return m_icmp_tx_seq;}
+    uint16_t get_icmp_rx_seq() {return m_icmp_rx_seq;}
+
+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_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
+     // ICMP seq num of next packet we will transmit
+     uint16_t m_icmp_tx_seq;
+     // ICMP seq num of last request we got
+     uint16_t m_icmp_rx_seq;
+     uint16_t pad1[1];
+
+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;
+     uint64_t m_no_id;
+     uint64_t m_seq_error;
+     uint64_t m_rx_check;
+     uint64_t m_no_ipv4_option;
+
+
+     uint64_t m_length_error;
+     CTimeHistogram  m_hist; /* all window */
+     CJitter         m_jitter; 
+};
+
+
+class CPortLatencyHWBase {
+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, 
+                               uint16_t nb_pkts){
+        return(0);
+    }
+};
+
+
+class CLatencyManagerCfg {
+public:
+    CLatencyManagerCfg (){
+        m_max_ports=0;
+        m_cps=0.0;
+        m_client_ip.v4=0x10000000;
+        m_server_ip.v4=0x20000000;
+        m_dual_port_mask=0x01000000;
+    }
+    uint32_t             m_max_ports;
+    double               m_cps;// CPS
+    CPortLatencyHWBase * m_ports[MAX_LATENCY_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;
+    CLatencyPktMode(uint8_t submode) {m_submode = submode;}
+    virtual uint8_t getPacketLen() = 0;
+    virtual const uint8_t *getPacketData() = 0;
+    virtual void rcv_debug_print(uint8_t *pkt) = 0;
+    virtual void send_debug_print(uint8_t *pkt) = 0;
+    virtual void update_pkt(uint8_t *pkt, bool is_client_to_server, uint16_t l4_len, uint16_t *tx_seq) = 0;
+    virtual bool IsLatencyPkt(IPHeader *ip) = 0;
+    uint8_t l4_header_len() {return 8;}
+    virtual void update_recv(uint8_t *pkt, uint16_t *r_seq, uint16_t *t_seq) = 0;
+    virtual uint8_t getProtocol() = 0;
+    virtual ~CLatencyPktMode() {}
+};
+
+class CLatencyPktModeICMP: public CLatencyPktMode {
+ public:
+    CLatencyPktModeICMP(uint8_t submode) : CLatencyPktMode(submode) {}
+    uint8_t getPacketLen();
+    const uint8_t *getPacketData();
+    void rcv_debug_print(uint8_t *);
+    void send_debug_print(uint8_t *);
+    void update_pkt(uint8_t *pkt, bool is_client_to_server, uint16_t l4_len, uint16_t *tx_seq);
+    bool IsLatencyPkt(IPHeader *ip);
+    void update_recv(uint8_t *pkt, uint16_t *r_seq, uint16_t *t_seq);
+    uint8_t getProtocol() {return 0x1;}
+};
+
+class CLatencyPktModeSCTP: public CLatencyPktMode {
+ public:
+    CLatencyPktModeSCTP(uint8_t submode) : CLatencyPktMode(submode) {}
+    uint8_t getPacketLen();
+    const uint8_t *getPacketData();
+    void rcv_debug_print(uint8_t *);
+    void send_debug_print(uint8_t *);
+    void update_pkt(uint8_t *pkt, bool is_client_to_server, uint16_t l4_len, uint16_t *tx_seq);
+    bool IsLatencyPkt(IPHeader *ip);
+    void update_recv(uint8_t *pkt, uint16_t *r_seq, uint16_t *t_seq);
+    uint8_t getProtocol() {return 0x84;}
+};
+
+class CLatencyManager {
+public:
+    bool Create(CLatencyManagerCfg * cfg);
+    void Delete();
+    void  reset();
+    void  start(int iter);
+    void  stop();
+    bool  is_active();
+    void set_ip(uint32_t client_ip,
+                uint32_t server_ip,
+                uint32_t mask_dual_port){
+        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 DumpRxCheck(FILE *fd); // dump all
+    void DumpShortRxCheck(FILE *fd); // dump short histogram of latency 
+    void rx_check_dump_json(std::string & json);
+    uint16_t get_latency_header_offset(){
+        return ( m_pkt_gen.get_payload_offset() );
+    }
+    void update();
+    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){
+        m_port_mask=mask;
+    }
+    double get_max_latency(void);
+    double get_avr_latency(void);
+    bool   is_any_error();
+    uint64_t get_total_pkt();
+    uint64_t get_total_bytes();
+    CNatRxManager * get_nat_manager(){
+        return ( &m_nat_check_manager );
+    }
+    CLatencyPktMode *c_l_pkt_mode;
+
+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);
+    /* messages handlers */
+    void handle_latency_pkt_msg(uint8_t thread_id,
+                               CGenNodeLatencyPktInfo * msg);
+
+     pqueue_t                m_p_queue; /* priorty queue */
+     bool                    m_is_active;
+     CLatencyPktInfo         m_pkt_gen;
+     CLatencyManagerPerPort  m_ports[MAX_LATENCY_PORTS];
+     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 
+     uint32_t                m_port_mask;
+     uint32_t                m_max_ports;
+     RxCheckManager 	     m_rx_check_manager;
+     CNatRxManager           m_nat_check_manager;
+     CCpuUtlDp               m_cpu_dp_u;
+     CCpuUtlCp               m_cpu_cp_u;
+     volatile bool           m_do_stop __rte_cache_aligned ;
+};
+
+#endif
+
diff --git a/src/main_dpdk.cpp b/src/main_dpdk.cpp
index 3e5418b9..7fd5503f 100755
--- a/src/main_dpdk.cpp
+++ b/src/main_dpdk.cpp
@@ -51,6 +51,7 @@ limitations under the License.
 #include <rte_mbuf.h>
 #include <rte_random.h>
 #include "bp_sim.h"
+#include "latency.h"
 #include "os_time.h"
 #include <common/arg/SimpleGlob.h>
 #include <common/arg/SimpleOpt.h>
@@ -77,6 +78,7 @@ extern "C" {
 #include "utl_term_io.h"
 #include "msg_manager.h"
 #include "platform_cfg.h"
+#include "latency.h"
 
 #include <internal_api/trex_platform_api.h>
 
@@ -118,13 +120,10 @@ static inline int get_is_latency_thread_enable(){
     return (CGlobalInfo::m_options.is_latency_enabled() ?1:0);
 }
 
-
-
 struct port_cfg_t;
 class  CPhyEthIF;
 class CPhyEthIFStats ;
 
-
 class CTRexExtendedDriverBase {
 public:
 
@@ -436,12 +435,6 @@ static inline int get_min_sample_rate(void){
     return ( get_ex_drv()->get_min_sample_rate());
 }
 
-
-
-
-
-
-
 #define MAX_DPDK_ARGS 40
 static CPlatformYamlInfo global_platform_cfg_info;
 static int global_dpdk_args_num ;
@@ -450,9 +443,6 @@ static char global_cores_str[100];
 static char global_prefix_str[100];
 static char global_loglevel_str[20];
 
-
-
-
 // cores =0==1,1*2,2,3,4,5,6
 // An enum for all the option types
 enum { OPT_HELP, 
@@ -487,6 +477,7 @@ enum { OPT_HELP,
     OPT_IPV6,
     OPT_LEARN,
     OPT_LEARN_VERIFY,
+    OPT_L_PKT_MODE,
     OPT_NO_FLOW_CONTROL,
     OPT_RX_CHECK_HOPS,
 	OPT_MAC_FILE,
@@ -499,9 +490,6 @@ enum { OPT_HELP,
 };
 
 
-
-      
-
 /* these are the argument types:
    SO_NONE --    no argument needed
    SO_REQ_SEP -- single required argument
@@ -549,6 +537,7 @@ static CSimpleOpt::SOption parser_options[] =
     { OPT_IPV6,       "--ipv6",       SO_NONE   },
     { OPT_LEARN, "--learn",       SO_NONE   },
     { OPT_LEARN_VERIFY, "--learn-verify",       SO_NONE   },
+    { OPT_L_PKT_MODE, "--l-pkt-mode",       SO_REQ_SEP   },
     { OPT_NO_FLOW_CONTROL, "--no-flow-control",       SO_NONE   },
     { OPT_VLAN,       "--vlan",       SO_NONE   },
     { OPT_MAC_FILE, "--mac", SO_REQ_SEP }, 
@@ -622,7 +611,11 @@ static int usage(){
     printf(" --learn                    : Work in NAT environments, learn the dynamic NAT translation and ALG  \n");
     printf(" --learn-verify             : Learn the translation, but intended for verification of the mechanism in cases that NAT does not exist \n");
     printf("  \n");
-    
+    printf(" --l-pkt-mode [0-3]         : Set mode for sending latency packets.\n");
+    printf("      0 (default)    send SCTP packets  \n");
+    printf("      1              Send ICMP request packets  \n");
+    printf("      2              Send ICMP requests from client side, and response from server side (for working with firewall) \n");
+    printf("      3              Send ICMP requests with sequence ID 0 from both sides \n");
     printf(" -v  [1-3]                  :  verbose mode ( works only on the debug image ! )  \n");
     printf("      1    show only stats  \n");
     printf("      2    run preview do not write to file  \n");
@@ -782,6 +775,11 @@ static int parse_options(int argc, char *argv[], CParserOption* po, bool first_t
                 po->preview.set_lean_and_verify_mode_enable(true);
                 break;
 
+            case OPT_L_PKT_MODE :
+                sscanf(args.OptionArg(),"%d", &tmp_data);
+                po->m_l_pkt_mode=(uint8_t)tmp_data;
+                break;
+
             case OPT_REAL_TIME  :
                 printf(" warning -r is deprecated, real time is not needed any more , it is the default \n");
                 po->preview.setRealTime(true);
@@ -1448,11 +1446,11 @@ void CPhyEthIF::configure(uint16_t nb_rx_queue,
 
 /*
 
-rx-queue 0 - default- all traffic no SCTP 
+rx-queue 0 - default- all traffic not goint to queue 1
              will be drop as queue is disable 
              
 
-rx-queue 1 - SCTP traffic will go to here  
+rx-queue 1 - Latency measurement packets will go here  
 
             pci_reg_write(IXGBE_L34T_IMIR(0),(1<<21));
 
@@ -1918,8 +1916,6 @@ public:
     virtual int send_node(CGenNode * node);
 };
 
-
-
 bool CCoreEthIF::Create(uint8_t             core_id,
                         uint16_t            tx_client_queue_id,
                         CPhyEthIF  *        tx_client_port, 
@@ -1939,54 +1935,6 @@ bool CCoreEthIF::Create(uint8_t             core_id,
     return (true);
 }
 
-bool CCoreEthIF::process_rx_pkt(pkt_dir_t   dir,
-                                rte_mbuf_t * m){
-
-    CSimplePacketParser parser(m);
-    if ( !parser.Parse()  ){
-        return false;
-    }
-    bool send=false;
-    if ( parser.IsLatencyPkt() ){
-        send=true;
-
-    }else{
-        if ( get_is_rx_filter_enable() ){
-            uint8_t max_ttl = 0xff - get_rx_check_hops();
-            uint8_t pkt_ttl = parser.getTTl();
-            if ( (pkt_ttl==max_ttl) || (pkt_ttl==(max_ttl-1) ) ) {
-               send=true;
-            }
-        }
-    }
-
-
-    if (send) {
-        CGenNodeLatencyPktInfo * node=(CGenNodeLatencyPktInfo * )CGlobalInfo::create_node();
-        if ( node ) {
-            node->m_msg_type = CGenNodeMsgBase::LATENCY_PKT;
-            node->m_dir      = dir;
-            node->m_latency_offset = 0xdead;
-            node->m_pkt      = m;
-            if ( m_ring_to_rx->Enqueue((CGenNode*)node)==0 ){
-            }else{
-                CGlobalInfo::free_node((CGenNode *)node);
-                send=false;
-            }
-
-            #ifdef LATENCY_QUEUE_TRACE_
-            printf("rx to cp --\n");
-            rte_pktmbuf_dump(stdout,m, rte_pktmbuf_pkt_len(m));
-            #endif
-        }else{
-            send=false;
-        }
-    }
-   return (send);
-}
-
-
-
 void CCoreEthIF::flush_rx_queue(void){
     pkt_dir_t   dir ;
     bool is_latency=get_is_latency_thread_enable();
@@ -2020,7 +1968,6 @@ void CCoreEthIF::flush_rx_queue(void){
     }
 }
 
-
 int CCoreEthIF::flush_tx_queue(void){
     /* flush both sides */
     pkt_dir_t   dir ;
@@ -2040,7 +1987,6 @@ int CCoreEthIF::flush_tx_queue(void){
     return (0);
 }
 
-
 void CCoreEthIF::GetCoreCounters(CVirtualIFPerSideStats *stats){
     stats->Clear();
     pkt_dir_t   dir ;
@@ -2854,7 +2800,7 @@ private:
 
     int create_pkt(uint8_t *pkt,int pkt_size);
     int create_udp_pkt();
-    int create_sctp_pkt();
+    int create_icmp_pkt();
 
 
 
@@ -2972,7 +2918,6 @@ int CGlobalTRex::test_send(){
     int i;
 
     //set_promisc_all(true);
-    //create_sctp_pkt();
     create_udp_pkt();
 
 	CRx_check_header rx_check_header;
@@ -2995,18 +2940,7 @@ int CGlobalTRex::test_send(){
         //test_send_pkts(m_latency_tx_queue_id,1,2);
         //test_send_pkts(m_latency_tx_queue_id,1,3);
         test_send_pkts(0,1,0);
-		test_send_pkts(0,1,1);
-        //test_send_pkts(2,1,0);
-
-
-        //test_send_pkts(0,1,1);
-        //test_send_pkts(0,1,2);
-        //test_send_pkts(0,1,3);
-
-        /*test_send_pkts(2,1,0);
-        test_send_pkts(2,1,1);
-        test_send_pkts(2,1,2);
-        test_send_pkts(2,1,3);*/
+        test_send_pkts(0,2,1);
 
         /*delay(1000);
         fprintf(stdout," --------------------------------\n");
@@ -3091,22 +3025,21 @@ const uint8_t udp_pkt[]={
 };
 
 
-const uint8_t sctp_pkt1[]={ 
-
+const uint8_t icmp_pkt1[]={
     0x00,0x00,0x00,0x01,0x00,0x00,
     0x00,0x00,0x00,0x01,0x00,0x00,
     0x08,0x00,
 
     0x45,0x02,0x00,0x30,
     0x00,0x00,0x40,0x00,
-    0x40,0x84,0xbd,0x04,
-    0x01,0x01,0x01,0x01, //sIP
-    0x02,0x02,0x02,0x02, //DIP
-
-    0x80,0x44,//SPORT
-    0x00,0x50,//DPORT
+    0xaa,0x01,0xbd,0x04,
+    0x9b,0xe6,0x18,0x9b, //SIP
+    0xcb,0xff,0xfc,0xc2, //DIP
 
-    0x00,0x00,0x00,0x00, //checksum 
+    0x08, 0x00, 
+    0x01, 0x02,  //checksum
+    0xaa, 0xbb,  // id
+    0x00, 0x00,  // Sequence number
 
     0x11,0x22,0x33,0x44, // magic 
     0x00,0x00,0x00,0x00, //64 bit counter
@@ -3114,8 +3047,7 @@ const uint8_t sctp_pkt1[]={
     0x00,0x01,0xa0,0x00, //seq
     0x00,0x00,0x00,0x00,
 
-};
-
+}; 
 
 
 
@@ -3144,8 +3076,8 @@ int CGlobalTRex::create_udp_pkt(){
     return (create_pkt((uint8_t*)udp_pkt,sizeof(udp_pkt)));
 }
 
-int CGlobalTRex::create_sctp_pkt(){
-    return (create_pkt((uint8_t*)sctp_pkt1,sizeof(sctp_pkt1)));
+int CGlobalTRex::create_icmp_pkt(){
+    return (create_pkt((uint8_t*)icmp_pkt1,sizeof(icmp_pkt1)));
 }
 
 
@@ -3392,7 +3324,7 @@ int  CGlobalTRex::ixgbe_start(void){
                           m_cores_to_dual_ports+1,
                           &m_port_cfg.m_port_conf);
 
-            /* the latency queue for SCTP */
+            /* the latency queue for latency measurement packets */
             m_latency_tx_queue_id= m_cores_to_dual_ports;
 
             socket_id_t socket_id = CGlobalInfo::m_socket.port_to_socket((port_id_t)i);
@@ -3409,7 +3341,7 @@ int  CGlobalTRex::ixgbe_start(void){
 
             /* set the filter queue */
             _if->set_rx_queue(1);
-            /* sctp ring is 1 */
+            /* latency measurement ring is 1 */
             _if->rx_queue_setup(1,
                                 RTE_TEST_RX_LATENCY_DESC_DEFAULT,
                                 socket_id, 
@@ -4351,6 +4283,54 @@ int CGlobalTRex::start_send_master(){
 
 static CGlobalTRex g_trex;
 
+bool CCoreEthIF::process_rx_pkt(pkt_dir_t   dir,
+                                rte_mbuf_t * m){
+
+    CSimplePacketParser parser(m);
+    if ( !parser.Parse()  ){
+        return false;
+    }
+    bool send=false;
+    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());
+
+    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();
+            if ( (pkt_ttl==max_ttl) || (pkt_ttl==(max_ttl-1) ) ) {
+               send=true;
+            }
+        }
+    }
+
+
+    if (send) {
+        CGenNodeLatencyPktInfo * node=(CGenNodeLatencyPktInfo * )CGlobalInfo::create_node();
+        if ( node ) {
+            node->m_msg_type = CGenNodeMsgBase::LATENCY_PKT;
+            node->m_dir      = dir;
+            node->m_latency_offset = 0xdead;
+            node->m_pkt      = m;
+            if ( m_ring_to_rx->Enqueue((CGenNode*)node)==0 ){
+            }else{
+                CGlobalInfo::free_node((CGenNode *)node);
+                send=false;
+            }
+
+            #ifdef LATENCY_QUEUE_TRACE_
+            printf("rx to cp --\n");
+            rte_pktmbuf_dump(stdout,m, rte_pktmbuf_pkt_len(m));
+            #endif
+        }else{
+            send=false;
+        }
+    }
+   return (send);
+}
+
 
 TrexStateless * get_stateless_obj() {
     return g_trex.m_trex_stateless;
@@ -4476,8 +4456,7 @@ int update_global_info_from_platform_file(){
 
     CGlobalInfo::m_memory_cfg.set(cg->m_memory,mul);
     CGlobalInfo::m_memory_cfg.set_number_of_dp_cors(
-        CGlobalInfo::m_options.get_number_of_dp_cores_needed() );
-
+    CGlobalInfo::m_options.get_number_of_dp_cores_needed() );
     return (0);
 }
 
@@ -4736,15 +4715,19 @@ int CTRexExtendedDriverBase1G::wait_for_stable_link(){
 }
 
 int CTRexExtendedDriverBase1G::configure_drop_queue(CPhyEthIF * _if){
+    uint8_t protocol;
+    if (CGlobalInfo::m_options.m_l_pkt_mode == 0) {
+        protocol = 0x84;
+    } else {
+        protocol = 0x1;
+    }
+
     _if->pci_reg_write( E1000_RXDCTL(0) , 0);
 
     /* enable filter to pass packet to rx queue 1 */
-
     _if->pci_reg_write( E1000_IMIR(0), 0x00020000);
-
     _if->pci_reg_write( E1000_IMIREXT(0), 0x00081000);
-
-    _if->pci_reg_write( E1000_TTQF(0),   0x00000084 /* protocol */
+    _if->pci_reg_write( E1000_TTQF(0),   protocol
                                   | 0x00008100 /* enable */
                                   | 0xE0010000 /* RX queue is 1 */
                                     );
@@ -4772,11 +4755,13 @@ int CTRexExtendedDriverBase1G::configure_rx_filter_rules(CPhyEthIF * _if){
         */
         int i;
         // IPv4: bytes being compared are {TTL, Protocol}
-        uint16_t ff_rules_v4[4]={
+        uint16_t ff_rules_v4[6]={
             (uint16_t)(0xFF06 - v4_hops),
             (uint16_t)(0xFE11 - v4_hops),
             (uint16_t)(0xFF11 - v4_hops),
             (uint16_t)(0xFE06 - v4_hops),
+            (uint16_t)(0xFF01 - v4_hops),
+            (uint16_t)(0xFE01 - v4_hops),
         }  ;
         // IPv6: bytes being compared are {NextHdr, HopLimit}
         uint16_t ff_rules_v6[2]={
@@ -4917,24 +4902,30 @@ int CTRexExtendedDriverBase10G::configure_rx_filter_rules(CPhyEthIF * _if){
 
 
         // IPv4: bytes being compared are {TTL, Protocol}
-        uint16_t ff_rules_v4[4]={
+        uint16_t ff_rules_v4[6]={
             (uint16_t)(0xFF11 - v4_hops),
             (uint16_t)(0xFE11 - v4_hops),
             (uint16_t)(0xFF06 - v4_hops),
             (uint16_t)(0xFE06 - v4_hops),
+            (uint16_t)(0xFF01 - v4_hops),
+            (uint16_t)(0xFE01 - v4_hops),
         }  ;
         // IPv6: bytes being compared are {NextHdr, HopLimit}
-        uint16_t ff_rules_v6[4]={
+        uint16_t ff_rules_v6[6]={
+            (uint16_t)(0x3CFF - hops),
+            (uint16_t)(0x3CFE - hops),
             (uint16_t)(0x3CFF - hops),
             (uint16_t)(0x3CFE - hops),
             (uint16_t)(0x3CFF - hops),
             (uint16_t)(0x3CFE - hops),
         }  ;
-        const rte_l4type ff_rules_type[4]={
+        const rte_l4type ff_rules_type[6]={
             RTE_FDIR_L4TYPE_UDP,
             RTE_FDIR_L4TYPE_UDP,
             RTE_FDIR_L4TYPE_TCP,
-            RTE_FDIR_L4TYPE_TCP
+            RTE_FDIR_L4TYPE_TCP,
+            RTE_FDIR_L4TYPE_NONE,
+            RTE_FDIR_L4TYPE_NONE
         }  ;
 
         uint16_t *ff_rules;
@@ -4974,8 +4965,7 @@ int CTRexExtendedDriverBase10G::configure_rx_filter_rules(CPhyEthIF * _if){
 }
 
 int CTRexExtendedDriverBase10G::configure_drop_queue(CPhyEthIF * _if){
-
-        /* enable rule 0 SCTP -> queue 1 for latency  */
+    /* enable rule 0 SCTP -> queue 1 for latency  */
     /* 1<<21 means that queue 1 is for SCTP */
     _if->pci_reg_write(IXGBE_L34T_IMIR(0),(1<<21));
 
@@ -4985,7 +4975,6 @@ int CTRexExtendedDriverBase10G::configure_drop_queue(CPhyEthIF * _if){
                       ((0x0f)<<IXGBE_FTQF_5TUPLE_MASK_SHIFT)|IXGBE_FTQF_QUEUE_ENABLE);
 
     /* disable queue zero - default all traffic will go to here and will be dropped */
-
     _if->pci_reg_write( IXGBE_RXDCTL(0) , 0);
     return (0);
 }
@@ -5046,10 +5035,15 @@ void CTRexExtendedDriverBase40G::update_configuration(port_cfg_t * cfg){
 }
 
 
-
+/* Add rule to send packets with protocol 'type', and ttl 'ttl' to rx queue 1 */
 void CTRexExtendedDriverBase40G::add_rules(CPhyEthIF * _if,
                                            enum rte_eth_flow_type type,
                                            uint8_t ttl){
+    if (CGlobalInfo::m_options.m_l_pkt_mode != 0) {
+        printf("Currently, on 40G, only SCTP latency pkt mode is supported. Please remove --l-pkt-mode option.\n");
+        exit(-1);
+    }
+
     uint8_t port_id = _if->get_port_id();
     int ret=rte_eth_dev_filter_supported(port_id, RTE_ETH_FILTER_FDIR);
 
@@ -5100,6 +5094,8 @@ int CTRexExtendedDriverBase40G::configure_rx_filter_rules(CPhyEthIF * _if){
 
 int CTRexExtendedDriverBase40G::configure_drop_queue(CPhyEthIF * _if){
 
+    /* ??? support ICMP in the driver */
+    /* Configure queue for latency packets */
     add_rules(_if,RTE_ETH_FLOW_TYPE_SCTPV4,0);
     return (0);
 }