Add support for parsing PW Ethernet and Enhanced MPLS parsing

include/stellar/packet.h

@@ -12,36 +12,37 @@ enum layer_type
 {
     // L2 -- data link layer
     LAYER_TYPE_ETHER = 1 << 0,
-    LAYER_TYPE_PPP = 1 << 1,
+    LAYER_TYPE_PWETH = 1 << 1,
-    LAYER_TYPE_HDLC = 1 << 2,
+    LAYER_TYPE_PPP = 1 << 2,
-    LAYER_TYPE_L2TP = 1 << 3,
+    LAYER_TYPE_HDLC = 1 << 3,
-    LAYER_TYPE_L2 = (LAYER_TYPE_ETHER | LAYER_TYPE_PPP | LAYER_TYPE_HDLC | LAYER_TYPE_L2TP),
+    LAYER_TYPE_L2TP = 1 << 4,
+    LAYER_TYPE_L2 = (LAYER_TYPE_ETHER | LAYER_TYPE_PWETH | LAYER_TYPE_PPP | LAYER_TYPE_HDLC | LAYER_TYPE_L2TP),
 
     // L2 -- tunnel
-    LAYER_TYPE_VLAN = 1 << 4,
+    LAYER_TYPE_VLAN = 1 << 5,
-    LAYER_TYPE_PPPOE = 1 << 5,
+    LAYER_TYPE_PPPOE = 1 << 6,
-    LAYER_TYPE_MPLS = 1 << 6,
+    LAYER_TYPE_MPLS = 1 << 7,
     LAYER_TYPE_L2_TUN = (LAYER_TYPE_VLAN | LAYER_TYPE_PPPOE | LAYER_TYPE_MPLS),
 
     // L3 -- network layer
-    LAYER_TYPE_IPV4 = 1 << 7,
+    LAYER_TYPE_IPV4 = 1 << 8,
-    LAYER_TYPE_IPV6 = 1 << 8,
+    LAYER_TYPE_IPV6 = 1 << 9,
     LAYER_TYPE_L3 = (LAYER_TYPE_IPV4 | LAYER_TYPE_IPV6),
 
     // L3 -- tunnel
-    LAYER_TYPE_GRE = 1 << 9,
+    LAYER_TYPE_GRE = 1 << 10,
     LAYER_TYPE_L3_TUN = (LAYER_TYPE_GRE),
 
     // L4 -- transport layer
-    LAYER_TYPE_UDP = 1 << 10,
+    LAYER_TYPE_UDP = 1 << 11,
-    LAYER_TYPE_TCP = 1 << 11,
+    LAYER_TYPE_TCP = 1 << 12,
-    LAYER_TYPE_ICMP = 1 << 12,
+    LAYER_TYPE_ICMP = 1 << 13,
-    LAYER_TYPE_ICMP6 = 1 << 13,
+    LAYER_TYPE_ICMP6 = 1 << 14,
     LAYER_TYPE_L4 = (LAYER_TYPE_UDP | LAYER_TYPE_TCP | LAYER_TYPE_ICMP | LAYER_TYPE_ICMP6),
 
     // L4 -- tunnel
-    LAYER_TYPE_VXLAN = 1 << 14,
+    LAYER_TYPE_VXLAN = 1 << 15,
-    LAYER_TYPE_GTPV1_U = 1 << 15,
+    LAYER_TYPE_GTPV1_U = 1 << 16,
 
     // ALL
     LAYER_TYPE_ALL = (LAYER_TYPE_L2 | LAYER_TYPE_L2_TUN | LAYER_TYPE_L3 | LAYER_TYPE_L3_TUN | LAYER_TYPE_L4 | LAYER_TYPE_VXLAN | LAYER_TYPE_GTPV1_U),

src/packet/eth_utils.h

@@ -0,0 +1,290 @@
+#ifndef _ETH_UTILS_H
+#define _ETH_UTILS_H
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#include <stdio.h>
+#include <string.h>
+#include <arpa/inet.h>
+#include <netinet/ether.h>
+
+/******************************************************************************
+ * get
+ ******************************************************************************/
+
+static inline int eth_hdr_get_dest(const struct ethhdr *hdr, char *buff, int size)
+{
+    return snprintf(buff, size, "%02x:%02x:%02x:%02x:%02x:%02x",
+                    hdr->h_dest[0], hdr->h_dest[1], hdr->h_dest[2],
+                    hdr->h_dest[3], hdr->h_dest[4], hdr->h_dest[5]);
+}
+
+static inline int eth_hdr_get_source(const struct ethhdr *hdr, char *buff, int size)
+{
+    return snprintf(buff, size, "%02x:%02x:%02x:%02x:%02x:%02x",
+                    hdr->h_source[0], hdr->h_source[1], hdr->h_source[2],
+                    hdr->h_source[3], hdr->h_source[4], hdr->h_source[5]);
+}
+
+static inline uint16_t eth_hdr_get_proto(const struct ethhdr *hdr)
+{
+    return ntohs(hdr->h_proto);
+}
+
+/******************************************************************************
+ * set
+ ******************************************************************************/
+
+static inline void eth_hdr_set_dest(struct ethhdr *hdr, const char *dest)
+{
+    sscanf(dest, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
+           &hdr->h_dest[0], &hdr->h_dest[1], &hdr->h_dest[2],
+           &hdr->h_dest[3], &hdr->h_dest[4], &hdr->h_dest[5]);
+}
+
+static inline void eth_hdr_set_source(struct ethhdr *hdr, const char *source)
+{
+    sscanf(source, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
+           &hdr->h_source[0], &hdr->h_source[1], &hdr->h_source[2],
+           &hdr->h_source[3], &hdr->h_source[4], &hdr->h_source[5]);
+}
+
+static inline void eth_hdr_set_proto(struct ethhdr *hdr, uint16_t proto)
+{
+    hdr->h_proto = htons(proto);
+}
+
+/******************************************************************************
+ * identify
+ ******************************************************************************/
+
+// /usr/include/linux/if_ether.h.html
+static inline int is_eth_proto(uint16_t proto)
+{
+    switch (proto)
+    {
+    case ETH_P_LOOP:
+    case ETH_P_PUP:
+    case ETH_P_PUPAT:
+    case ETH_P_IP:
+    case ETH_P_X25:
+    case ETH_P_ARP:
+    case ETH_P_BPQ:
+    case ETH_P_IEEEPUP:
+    case ETH_P_IEEEPUPAT:
+    case ETH_P_DEC:
+    case ETH_P_DNA_DL:
+    case ETH_P_DNA_RC:
+    case ETH_P_DNA_RT:
+    case ETH_P_LAT:
+    case ETH_P_DIAG:
+    case ETH_P_CUST:
+    case ETH_P_SCA:
+    case ETH_P_TEB:
+    case ETH_P_RARP:
+    case ETH_P_ATALK:
+    case ETH_P_AARP:
+    case ETH_P_8021Q:
+    case ETH_P_IPX:
+    case ETH_P_IPV6:
+    case ETH_P_PAUSE:
+    case ETH_P_SLOW:
+    case ETH_P_WCCP:
+    case ETH_P_PPP_DISC:
+    case ETH_P_PPP_SES:
+    case ETH_P_MPLS_UC:
+    case ETH_P_MPLS_MC:
+    case ETH_P_ATMMPOA:
+    case ETH_P_ATMFATE:
+    case ETH_P_PAE:
+    case ETH_P_AOE:
+    case ETH_P_TIPC:
+    case ETH_P_1588:
+    case ETH_P_FCOE:
+    case ETH_P_FIP:
+    case ETH_P_EDSA:
+    case ETH_P_802_3:
+    case ETH_P_AX25:
+    case ETH_P_ALL:
+    case ETH_P_802_2:
+    case ETH_P_SNAP:
+    case ETH_P_DDCMP:
+    case ETH_P_WAN_PPP:
+    case ETH_P_PPP_MP:
+    case ETH_P_LOCALTALK:
+    case ETH_P_CAN:
+    case ETH_P_PPPTALK:
+    case ETH_P_TR_802_2:
+    case ETH_P_MOBITEX:
+    case ETH_P_CONTROL:
+    case ETH_P_IRDA:
+    case ETH_P_ECONET:
+    case ETH_P_HDLC:
+    case ETH_P_ARCNET:
+    case ETH_P_DSA:
+    case ETH_P_TRAILER:
+    case ETH_P_PHONET:
+    case ETH_P_IEEE802154:
+        return 1;
+    default:
+        return 0;
+    }
+}
+
+static inline const char *eth_proto_to_str(uint16_t proto)
+{
+    switch (proto)
+    {
+    case ETH_P_LOOP:
+        return "ETH_P_LOOP";
+    case ETH_P_PUP:
+        return "ETH_P_PUP";
+    case ETH_P_PUPAT:
+        return "ETH_P_PUPAT";
+    case ETH_P_IP:
+        return "ETH_P_IP";
+    case ETH_P_X25:
+        return "ETH_P_X25";
+    case ETH_P_ARP:
+        return "ETH_P_ARP";
+    case ETH_P_BPQ:
+        return "ETH_P_BPQ";
+    case ETH_P_IEEEPUP:
+        return "ETH_P_IEEEPUP";
+    case ETH_P_IEEEPUPAT:
+        return "ETH_P_IEEEPUPAT";
+    case ETH_P_DEC:
+        return "ETH_P_DEC";
+    case ETH_P_DNA_DL:
+        return "ETH_P_DNA_DL";
+    case ETH_P_DNA_RC:
+        return "ETH_P_DNA_RC";
+    case ETH_P_DNA_RT:
+        return "ETH_P_DNA_RT";
+    case ETH_P_LAT:
+        return "ETH_P_LAT";
+    case ETH_P_DIAG:
+        return "ETH_P_DIAG";
+    case ETH_P_CUST:
+        return "ETH_P_CUST";
+    case ETH_P_SCA:
+        return "ETH_P_SCA";
+    case ETH_P_TEB:
+        return "ETH_P_TEB";
+    case ETH_P_RARP:
+        return "ETH_P_RARP";
+    case ETH_P_ATALK:
+        return "ETH_P_ATALK";
+    case ETH_P_AARP:
+        return "ETH_P_AARP";
+    case ETH_P_8021Q:
+        return "ETH_P_8021Q";
+    case ETH_P_IPX:
+        return "ETH_P_IPX";
+    case ETH_P_IPV6:
+        return "ETH_P_IPV6";
+    case ETH_P_PAUSE:
+        return "ETH_P_PAUSE";
+    case ETH_P_SLOW:
+        return "ETH_P_SLOW";
+    case ETH_P_WCCP:
+        return "ETH_P_WCCP";
+    case ETH_P_PPP_DISC:
+        return "ETH_P_PPP_DISC";
+    case ETH_P_PPP_SES:
+        return "ETH_P_PPP_SES";
+    case ETH_P_MPLS_UC:
+        return "ETH_P_MPLS_UC";
+    case ETH_P_MPLS_MC:
+        return "ETH_P_MPLS_MC";
+    case ETH_P_ATMMPOA:
+        return "ETH_P_ATMMPOA";
+    case ETH_P_ATMFATE:
+        return "ETH_P_ATMFATE";
+    case ETH_P_PAE:
+        return "ETH_P_PAE";
+    case ETH_P_AOE:
+        return "ETH_P_AOE";
+    case ETH_P_TIPC:
+        return "ETH_P_TIPC";
+    case ETH_P_1588:
+        return "ETH_P_1588";
+    case ETH_P_FCOE:
+        return "ETH_P_FCOE";
+    case ETH_P_FIP:
+        return "ETH_P_FIP";
+    case ETH_P_EDSA:
+        return "ETH_P_EDSA";
+    case ETH_P_802_3:
+        return "ETH_P_802_3";
+    case ETH_P_AX25:
+        return "ETH_P_AX25";
+    case ETH_P_ALL:
+        return "ETH_P_ALL";
+    case ETH_P_802_2:
+        return "ETH_P_802_2";
+    case ETH_P_SNAP:
+        return "ETH_P_SNAP";
+    case ETH_P_DDCMP:
+        return "ETH_P_DDCMP";
+    case ETH_P_WAN_PPP:
+        return "ETH_P_WAN_PPP";
+    case ETH_P_PPP_MP:
+        return "ETH_P_PPP_MP";
+    case ETH_P_LOCALTALK:
+        return "ETH_P_LOCALTALK";
+    case ETH_P_CAN:
+        return "ETH_P_CAN";
+    case ETH_P_PPPTALK:
+        return "ETH_P_PPPTALK";
+    case ETH_P_TR_802_2:
+        return "ETH_P_TR_802_2";
+    case ETH_P_MOBITEX:
+        return "ETH_P_MOBITEX";
+    case ETH_P_CONTROL:
+        return "ETH_P_CONTROL";
+    case ETH_P_IRDA:
+        return "ETH_P_IRDA";
+    case ETH_P_ECONET:
+        return "ETH_P_ECONET";
+    case ETH_P_HDLC:
+        return "ETH_P_HDLC";
+    case ETH_P_ARCNET:
+        return "ETH_P_ARCNET";
+    case ETH_P_DSA:
+        return "ETH_P_DSA";
+    case ETH_P_TRAILER:
+        return "ETH_P_TRAILER";
+    case ETH_P_PHONET:
+        return "ETH_P_PHONET";
+    case ETH_P_IEEE802154:
+        return "ETH_P_IEEE802154";
+    default:
+        return "ETH_P_UNKNOWN";
+    }
+}
+
+/******************************************************************************
+ * print
+ ******************************************************************************/
+
+static inline int eth_hdr_to_str(const struct ethhdr *hdr, char *buf, size_t size)
+{
+    memset(buf, 0, size);
+    char dest[18] = {0};
+    char source[18] = {0};
+    uint16_t proto = eth_hdr_get_proto(hdr);
+    eth_hdr_get_dest(hdr, dest, sizeof(dest));
+    eth_hdr_get_source(hdr, source, sizeof(source));
+    return snprintf(buf, size, "ETH: source=%s dest=%s proto=%s",
+                    source, dest, eth_proto_to_str(proto));
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/packet/ipv4_utils.h

@@ -256,6 +256,10 @@ static inline void ipv4_hdr_set_opt_data(struct ip *hdr, const char *opt_data)
     memcpy((char *)hdr + sizeof(struct ip), opt_data, ipv4_hdr_get_opt_len(hdr));
 }
 
+/******************************************************************************
+ * print
+ ******************************************************************************/
+
 static inline int ipv4_hdr_to_str(const struct ip *hdr, char *buf, size_t size)
 {
     memset(buf, 0, size);

src/packet/ipv6_utils.h

@@ -197,6 +197,10 @@ static inline void ipv6_frag_set_more(struct ip6_frag *frag, bool more)
     }
 }
 
+/******************************************************************************
+ * print
+ ******************************************************************************/
+
 static inline int ipv6_hdr_to_str(const struct ip6_hdr *hdr, char *buf, size_t size)
 {
     memset(buf, 0, size);

src/packet/mpls_utils.h

@@ -0,0 +1,105 @@
+#ifndef _MPLS_UTILS_H
+#define _MPLS_UTILS_H
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#include <stdio.h>
+#include <string.h>
+#include <arpa/inet.h>
+
+#define MPLS_LABEL_MASK 0xFFFFF000
+#define MPLS_EXP_MASK 0x00000E00
+#define MPLS_STACK_MASK 0x00000100
+#define MPLS_TTL_MASK 0x000000FF
+
+#define MPLS_LABEL_SHIFT 12
+#define MPLS_EXP_SHIFT 9
+#define MPLS_STACK_SHIFT 8
+#define MPLS_TTL_SHIFT 0
+
+/* Reference: RFC 5462, RFC 3032
+ *
+ *  0                   1                   2                   3
+ *  0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |                Label                  | EXP |S|       TTL     |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *
+ *	Label:  Label Value, 20 bits
+ *	EXP:    Experimental, 3 bits
+ *	S:      Bottom of Stack, 1 bit
+ *	TTL:    Time to Live, 8 bits
+ */
+
+struct mpls_hdr
+{
+    uint32_t entry;
+};
+
+/******************************************************************************
+ * get
+ ******************************************************************************/
+
+static inline uint32_t mpls_hdr_get_label(const struct mpls_hdr *hdr)
+{
+    return ((ntohl(hdr->entry) & MPLS_LABEL_MASK) >> MPLS_LABEL_SHIFT);
+}
+
+static inline uint8_t mpls_hdr_get_exp(const struct mpls_hdr *hdr)
+{
+    return ((ntohl(hdr->entry) & MPLS_EXP_MASK) >> MPLS_EXP_SHIFT);
+}
+
+static inline uint8_t mpls_hdr_get_bos(const struct mpls_hdr *hdr)
+{
+    return ((ntohl(hdr->entry) & MPLS_STACK_MASK) >> MPLS_STACK_SHIFT);
+}
+
+static inline uint8_t mpls_hdr_get_ttl(const struct mpls_hdr *hdr)
+{
+    return ((ntohl(hdr->entry) & MPLS_TTL_MASK) >> MPLS_TTL_SHIFT);
+}
+
+/******************************************************************************
+ * set
+ ******************************************************************************/
+
+static inline void mpls_hdr_set_label(struct mpls_hdr *hdr, uint32_t label)
+{
+    hdr->entry = htonl((ntohl(hdr->entry) & ~MPLS_LABEL_MASK) | (label << MPLS_LABEL_SHIFT));
+}
+
+static inline void mpls_hdr_set_exp(struct mpls_hdr *hdr, uint8_t exp)
+{
+    hdr->entry = htonl((ntohl(hdr->entry) & ~MPLS_EXP_MASK) | (exp << MPLS_EXP_SHIFT));
+}
+
+static inline void mpls_hdr_set_bos(struct mpls_hdr *hdr, uint8_t bos)
+{
+    hdr->entry = htonl((ntohl(hdr->entry) & ~MPLS_STACK_MASK) | (bos << MPLS_STACK_SHIFT));
+}
+
+static inline void mpls_hdr_set_ttl(struct mpls_hdr *hdr, uint8_t ttl)
+{
+    hdr->entry = htonl((ntohl(hdr->entry) & ~MPLS_TTL_MASK) | (ttl << MPLS_TTL_SHIFT));
+}
+
+/******************************************************************************
+ * print
+ ******************************************************************************/
+
+static inline int mpls_hdr_to_str(const struct mpls_hdr *hdr, char *buf, size_t size)
+{
+    return snprintf(buf, size, "MPLS: label=%u exp=%u bos=%u ttl=%u",
+                    mpls_hdr_get_label(hdr), mpls_hdr_get_exp(hdr),
+                    mpls_hdr_get_bos(hdr), mpls_hdr_get_ttl(hdr));
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/packet/packet.cpp

@@ -1,11 +1,5 @@
 #include <string.h>
 #include <stdlib.h>
-#include <netinet/ip.h>
-#include <netinet/ip6.h>
-#define __FAVOR_BSD 1
-#include <netinet/tcp.h>
-#include <netinet/udp.h>
-#include <netinet/ether.h>
 #include <netinet/icmp6.h>
 #include <netinet/ip_icmp.h>
 #include <linux/ppp_defs.h>
@@ -16,6 +10,8 @@
 #include "tcp_utils.h"
 #include "ipv4_utils.h"
 #include "ipv6_utils.h"
+#include "mpls_utils.h"
+#include "eth_utils.h"
 
 #define likely(expr) __builtin_expect((expr), 1)
 #define unlikely(expr) __builtin_expect((expr), 0)
@@ -35,7 +31,7 @@
 #define PACKET_LOG_UNSUPPORT_ETHPROTO(tag, next_proto)                      \
     {                                                                       \
         PACKET_LOG_WARN("%s: unsupport next eth proto %d: %s",              \
-                        (tag), (next_proto), ethproto_to_str(next_proto)); \
+                        (tag), (next_proto), eth_proto_to_str(next_proto)); \
     }
 
 #define PACKET_LOG_UNSUPPORT_IPPROTO(tag, next_proto)                     \
@@ -48,7 +44,6 @@
  * Static API
  ******************************************************************************/
 
-static const char *ethproto_to_str(uint16_t proto);
 static const char *ipproto_to_str(uint16_t proto);
 static inline const char *ldbc_method_to_str(enum ldbc_method method);
 static inline const char *layer_type_to_str(enum layer_type type);
@@ -65,6 +60,7 @@ static inline uint16_t get_l2tpv2_hdr_len(const char *data, uint16_t len);
 
 // 数据链路层
 static inline const char *parse_ether(struct packet *pkt, const char *data, uint16_t len);
+static inline const char *parse_pweth(struct packet *pkt, const char *data, uint16_t len);
 static inline const char *parse_ppp(struct packet *pkt, const char *data, uint16_t len);
 static inline const char *parse_l2tpv2(struct packet *pkt, const char *data, uint16_t len);
 static inline const char *parse_l2tpv3_over_udp(struct packet *pkt, const char *data, uint16_t len);
@@ -95,113 +91,6 @@ static inline const char *parse_l4(struct packet *pkt, uint8_t next_proto, const
  * Private API -- Utils
  ******************************************************************************/
 
-static const char *ethproto_to_str(uint16_t proto)
-{
-    switch (proto)
-    {
-    case ETH_P_LOOP:
-        return "LOOP";
-    case ETH_P_PUP:
-        return "PUP";
-    case ETH_P_PUPAT:
-        return "PUPAT";
-    case ETH_P_IP:
-        return "IP";
-    case ETH_P_X25:
-        return "X25";
-    case ETH_P_ARP:
-        return "ARP";
-    case ETH_P_BPQ:
-        return "BPQ";
-    case ETH_P_IEEEPUP:
-        return "IEEEPUP";
-    case ETH_P_IEEEPUPAT:
-        return "IEEEPUPAT";
-    case ETH_P_DEC:
-        return "DEC";
-    case ETH_P_DNA_DL:
-        return "DNA_DL";
-    case ETH_P_DNA_RC:
-        return "DNA_RC";
-    case ETH_P_DNA_RT:
-        return "DNA_RT";
-    case ETH_P_LAT:
-        return "LAT";
-    case ETH_P_DIAG:
-        return "DIAG";
-    case ETH_P_CUST:
-        return "CUST";
-    case ETH_P_SCA:
-        return "SCA";
-    case ETH_P_TEB:
-        return "TEB";
-    case ETH_P_RARP:
-        return "RARP";
-    case ETH_P_ATALK:
-        return "ATALK";
-    case ETH_P_AARP:
-        return "AARP";
-    case ETH_P_8021Q:
-        return "8021Q";
-    case ETH_P_IPX:
-        return "IPX";
-    case ETH_P_IPV6:
-        return "IPV6";
-    case ETH_P_PAUSE:
-        return "PAUSE";
-    case ETH_P_SLOW:
-        return "SLOW";
-    case ETH_P_WCCP:
-        return "WCCP";
-    case ETH_P_PPP_DISC:
-        return "PPP_DISC";
-    case ETH_P_PPP_SES:
-        return "PPP_SES";
-    case ETH_P_MPLS_UC:
-        return "MPLS_UC";
-    case ETH_P_MPLS_MC:
-        return "MPLS_MC";
-    case ETH_P_ATMMPOA:
-        return "ATMMPOA";
-    case ETH_P_LINK_CTL:
-        return "LINK_CTL";
-    case ETH_P_ATMFATE:
-        return "ATMFATE";
-    case ETH_P_PAE:
-        return "PAE";
-    case ETH_P_AOE:
-        return "AOE";
-    case ETH_P_8021AD:
-        return "8021AD";
-    case ETH_P_802_EX1:
-        return "802_EX1";
-    case ETH_P_TIPC:
-        return "TIPC";
-    case ETH_P_8021AH:
-        return "8021AH";
-    case ETH_P_1588:
-        return "1588";
-    case ETH_P_FCOE:
-        return "FCOE";
-    case ETH_P_TDLS:
-        return "TDLS";
-    case ETH_P_FIP:
-        return "FIP";
-    case ETH_P_QINQ1:
-        return "QINQ1";
-    case ETH_P_QINQ2:
-        return "QINQ2";
-    case ETH_P_QINQ3:
-        return "QINQ3";
-    case ETH_P_EDSA:
-        return "EDSA";
-    case ETH_P_AF_IUCV:
-        return "AF_IUCV";
-    default:
-        return "UNKNOWN";
-    }
-}
-
 static const char *ipproto_to_str(uint16_t proto)
 {
     switch (proto)
@@ -292,6 +181,8 @@ static inline const char *layer_type_to_str(enum layer_type type)
     {
     case LAYER_TYPE_ETHER:
         return "ETH";
+    case LAYER_TYPE_PWETH:
+        return "PWETH";
     case LAYER_TYPE_PPP:
         return "PPP";
     case LAYER_TYPE_HDLC:
@@ -764,17 +655,43 @@ static inline const char *parse_ether(struct packet *pkt, const char *data, uint
     {
         return data;
     }
-    uint16_t next_proto = ntohs(((struct ethhdr *)data)->h_proto);
+    uint16_t next_proto = eth_hdr_get_proto((const struct ethhdr *)data);
     SET_LAYER(pkt, layer, LAYER_TYPE_ETHER, sizeof(struct ethhdr), data, len, 0);
 
-    // TESTED
     return parse_l3(pkt, next_proto, layer->pld_ptr, layer->pld_len);
 }
 
-static inline int next_proto_is_ppp(uint16_t next_proto)
+static inline const char *parse_pweth(struct packet *pkt, const char *data, uint16_t len)
+{
+    /*
+     * PW Ethernet Control Word
+     *  0                   1                   2                   3
+     *  0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+     * |0 0 0 0|   Reserved            |       Sequence Number         |
+     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+     * Reference: https://tools.ietf.org/html/rfc4448
+     */
+    if (unlikely(len < 4))
+    {
+        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_PWETH);
+        return data;
+    }
+
+    struct packet_layer *layer = get_free_layer(pkt);
+    if (unlikely(layer == NULL))
+    {
+        return data;
+    }
+    SET_LAYER(pkt, layer, LAYER_TYPE_PWETH, 4, data, len, 0);
+
+    return parse_ether(pkt, layer->pld_ptr, layer->pld_len);
+}
+
+static inline int is_ppp_proto(uint16_t proto)
 {
     // /usr/include/linux/ppp_defs.h.html
-    switch (next_proto)
+    switch (proto)
     {
     case PPP_IP:         /* Internet Protocol */
     case PPP_AT:         /* AppleTalk Protocol */
@@ -835,7 +752,7 @@ static inline const char *parse_ppp(struct packet *pkt, const char *data, uint16
 
     // ppp header 1 byte
     next_proto = *((uint8_t *)data);
-    if (next_proto_is_ppp(next_proto))
+    if (is_ppp_proto(next_proto))
     {
         hdr_len = 1;
         goto success;
@@ -843,7 +760,7 @@ static inline const char *parse_ppp(struct packet *pkt, const char *data, uint16
 
     // ppp header 2 bytes
     next_proto = ntohs(*((uint16_t *)data));
-    if (next_proto_is_ppp(next_proto))
+    if (is_ppp_proto(next_proto))
     {
         hdr_len = 2;
         goto success;
@@ -862,7 +779,6 @@ success:
     SET_LAYER(pkt, layer, LAYER_TYPE_PPP, hdr_len, data, len, 0);
     switch (next_proto)
     {
-        // TESTED
     case PPP_IP:
         return parse_ipv4(pkt, layer->pld_ptr, layer->pld_len);
     case PPP_IPV6:
@@ -936,13 +852,11 @@ static inline const char *parse_vlan(struct packet *pkt, const char *data, uint1
     uint16_t next_proto = ntohs(((struct vlan_hdr *)data)->protocol);
     SET_LAYER(pkt, layer, LAYER_TYPE_VLAN, sizeof(struct vlan_hdr), data, len, 0);
 
-    // TESTED
     return parse_l3(pkt, next_proto, layer->pld_ptr, layer->pld_len);
 }
 
 static inline const char *parse_pppoe_ses(struct packet *pkt, const char *data, uint16_t len)
 {
-
     if (unlikely(len < 6))
     {
         PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_PPPOE);
@@ -956,40 +870,12 @@ static inline const char *parse_pppoe_ses(struct packet *pkt, const char *data,
     }
     SET_LAYER(pkt, layer, LAYER_TYPE_PPPOE, 6, data, len, 0);
 
-    // TESTED
     return parse_ppp(pkt, layer->pld_ptr, layer->pld_len);
 }
 
 static inline const char *parse_mpls(struct packet *pkt, const char *data, uint16_t len)
 {
-    /*
+    if (unlikely(len < 4))
-     * MPLS Format
-     * 0                   1                   2                   3
-     * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
-     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
-     * |                Label                  | Exp |S|       TTL     |
-     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
-     * Label : Label Value      20 bits
-     * Exp   : Experimental Use  3 bits
-     * S     : Bottom of Stack   1 bit
-     * TTL   : Time to Live      8 bits
-     */
-
-#define MPLS_LABEL_MASK (0xFFFFF000)
-#define MPLS_EXP_MASK (0x00000E00)
-#define MPLS_BLS_MASK (0x00000100)
-#define MPLS_TTL_MASK (0x000000FF)
-
-    enum mpls_next_proto
-    {
-        MPLS_NEXT_PROTO_ETHER = 0x0,
-        MPLS_NEXT_PROTO_MPLS = 0x1,
-        MPLS_NEXT_PROTO_IPV4 = 0x4,
-        MPLS_NEXT_PROTO_IPV6 = 0x6,
-    };
-
-    // 4 + 1
-    if (unlikely(len < 5))
     {
         PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_MPLS);
         return data;
@@ -1001,65 +887,38 @@ static inline const char *parse_mpls(struct packet *pkt, const char *data, uint1
         return data;
     }
 
-    uint16_t hdr_len = 4;
+    if (mpls_hdr_get_bos((const struct mpls_hdr *)data))
-    uint32_t *hdr = (uint32_t *)data;
-    unsigned int mpls_bls = (ntohl(*hdr) & MPLS_BLS_MASK) >> 8;
-    enum mpls_next_proto next_proto;
-    if (mpls_bls == 1)
     {
-        switch ((((uint8_t *)(data + 4))[0]) >> 4)
+        SET_LAYER(pkt, layer, LAYER_TYPE_MPLS, 4, data, len, 0);
+        if (layer->pld_len == 0)
+        {
+            return layer->pld_ptr;
+        }
+        uint8_t next_proto = layer->pld_ptr[0] >> 4;
+        switch (next_proto)
         {
         case 0:
-            /*
+            // the first four digits of the PW Ethernet control word must be "00000", but the first four digits of Ethernet may also be "0000"
-             * PW Ethernet Control Word
+            if (layer->pld_len >= sizeof(struct ethhdr) && is_eth_proto(eth_hdr_get_proto((const struct ethhdr *)layer->pld_ptr)))
-             * 0                   1                   2                   3
+            {
-             * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+                return parse_ether(pkt, layer->pld_ptr, layer->pld_len);
-             * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+            }
-             * |0 0 0 0|   Reserved            |       Sequence Number         |
+            else
-             * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+            {
-             * Reference: https://tools.ietf.org/html/rfc4448
+                return parse_pweth(pkt, layer->pld_ptr, layer->pld_len);
-             */
+            }
-            hdr_len += 4; // skip PW Ethernet Control Word
-            next_proto = MPLS_NEXT_PROTO_ETHER;
-            break;
         case 4:
-            next_proto = MPLS_NEXT_PROTO_IPV4;
+            return parse_ipv4(pkt, layer->pld_ptr, layer->pld_len);
-            break;
         case 6:
-            next_proto = MPLS_NEXT_PROTO_IPV6;
+            return parse_ipv6(pkt, layer->pld_ptr, layer->pld_len);
-            break;
         default:
-            next_proto = MPLS_NEXT_PROTO_ETHER;
+            return parse_ether(pkt, layer->pld_ptr, layer->pld_len);
-            break;
         }
     }
     else
     {
-        next_proto = MPLS_NEXT_PROTO_MPLS;
+        SET_LAYER(pkt, layer, LAYER_TYPE_MPLS, 4, data, len, 0);
-    }
-    if (unlikely(hdr_len > len))
-    {
-        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_MPLS);
-        return data;
-    }
-    SET_LAYER(pkt, layer, LAYER_TYPE_MPLS, hdr_len, data, len, 0);
-
-    switch (next_proto)
-    {
-        // TESTED
-    case MPLS_NEXT_PROTO_IPV4:
-        return parse_ipv4(pkt, layer->pld_ptr, layer->pld_len);
-    case MPLS_NEXT_PROTO_IPV6:
-        return parse_ipv6(pkt, layer->pld_ptr, layer->pld_len);
-        // TESTED
-    case MPLS_NEXT_PROTO_ETHER:
-        return parse_ether(pkt, layer->pld_ptr, layer->pld_len);
-        // TESTED
-    case MPLS_NEXT_PROTO_MPLS:
         return parse_mpls(pkt, layer->pld_ptr, layer->pld_len);
-    default:
-        // unreachable
-        return layer->pld_ptr;
     }
 }
 
@@ -1076,7 +935,7 @@ static inline const char *parse_ipv4(struct packet *pkt, const char *data, uint1
     {
         return data;
     }
-    struct ip *hdr = (struct ip *)data;
+    const struct ip *hdr = (const struct ip *)data;
     uint16_t hdr_len = ipv4_hdr_get_hdr_len(hdr);
     if (unlikely(hdr_len > len))
     {
@@ -1101,7 +960,6 @@ static inline const char *parse_ipv4(struct packet *pkt, const char *data, uint1
         // try continue parse
     }
 
-    // TESTED
     uint8_t next_proto = ipv4_hdr_get_proto(hdr);
     return parse_l4(pkt, next_proto, layer->pld_ptr, layer->pld_len);
 }
@@ -1138,13 +996,14 @@ static inline const char *parse_ipv6(struct packet *pkt, const char *data, uint1
     {
         return data;
     }
-    uint16_t pld_len = ipv6_hdr_get_payload_len((const struct ip6_hdr *)data);
+    const struct ip6_hdr *hdr = (const struct ip6_hdr *)data;
+    uint16_t pld_len = ipv6_hdr_get_payload_len(hdr);
     if (unlikely(pld_len + sizeof(struct ip6_hdr) > len))
     {
         PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_IPV6);
         return data;
     }
-    uint8_t next_proto = ipv6_hdr_get_next_header((const struct ip6_hdr *)data);
+    uint8_t next_proto = ipv6_hdr_get_next_header(hdr);
     uint16_t hdr_len = sizeof(struct ip6_hdr);
     uint16_t trim_len = len - pld_len - sizeof(struct ip6_hdr);
     const char *next_hdr_ptr = data + hdr_len;
@@ -1177,7 +1036,6 @@ static inline const char *parse_ipv6(struct packet *pkt, const char *data, uint1
         // try continue parse
     }
 
-    // TESTED
     return parse_l4(pkt, next_proto, layer->pld_ptr, layer->pld_len);
 }
 
@@ -1209,7 +1067,6 @@ static inline const char *parse_gre(struct packet *pkt, const char *data, uint16
         return parse_ipv4(pkt, layer->pld_ptr, layer->pld_len);
     case GRE_PRO_IPV6:
         return parse_ipv6(pkt, layer->pld_ptr, layer->pld_len);
-        // TESTED
     case GRE_PRO_PPP:
         return parse_ppp(pkt, layer->pld_ptr, layer->pld_len);
     default:
@@ -1231,19 +1088,17 @@ static inline const char *parse_udp(struct packet *pkt, const char *data, uint16
     {
         return data;
     }
-    struct udphdr *hdr = (struct udphdr *)data;
+    const struct udphdr *hdr = (struct udphdr *)data;
     SET_LAYER(pkt, layer, LAYER_TYPE_UDP, sizeof(struct udphdr), data, len, 0);
 
     if (udp_hdr_get_dst_port(hdr) == 4789)
     {
-        // TESTED
         // VXLAN_DPORT 4789
         return parse_vxlan(pkt, layer->pld_ptr, layer->pld_len);
     }
 
     if (udp_hdr_get_dst_port(hdr) == 2152 || udp_hdr_get_src_port(hdr) == 2152)
     {
-        // TESTED
         // GTP1U_PORT 2152
         return parse_gtpv1_u(pkt, layer->pld_ptr, layer->pld_len);
     }
@@ -1260,7 +1115,6 @@ static inline const char *parse_udp(struct packet *pkt, const char *data, uint16
         switch (L2TP_VERSION(control))
         {
         case 2:
-            // TESTED
             return parse_l2tpv2(pkt, layer->pld_ptr, layer->pld_len);
         case 3:
             return parse_l2tpv3_over_udp(pkt, layer->pld_ptr, layer->pld_len);
@@ -1285,7 +1139,7 @@ static inline const char *parse_tcp(struct packet *pkt, const char *data, uint16
     {
         return data;
     }
-    uint16_t hdr_len = tcp_hdr_get_hdr_len((struct tcphdr *)data);
+    uint16_t hdr_len = tcp_hdr_get_hdr_len((const struct tcphdr *)data);
     if (unlikely(hdr_len > len))
     {
         PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_TCP);
@@ -1355,7 +1209,6 @@ static inline const char *parse_vxlan(struct packet *pkt, const char *data, uint
     }
     SET_LAYER(pkt, layer, LAYER_TYPE_VXLAN, sizeof(struct vxlan_hdr), data, len, 0);
 
-    // TESTED
     return parse_ether(pkt, layer->pld_ptr, layer->pld_len);
 }
 
@@ -1378,10 +1231,8 @@ static inline const char *parse_gtpv1_u(struct packet *pkt, const char *data, ui
 
     switch (next_proto)
     {
-        // TESTED
     case 4:
         return parse_ipv4(pkt, layer->pld_ptr, layer->pld_len);
-        // TESTED
     case 6:
         return parse_ipv6(pkt, layer->pld_ptr, layer->pld_len);
     default:
@@ -1394,24 +1245,18 @@ static inline const char *parse_l3(struct packet *pkt, uint16_t next_proto, cons
 {
     switch (next_proto)
     {
-        // TESTED
     case ETH_P_8021Q:
     case ETH_P_8021AD:
         return parse_vlan(pkt, data, len);
-        // TESTED
     case ETH_P_IP:
         return parse_ipv4(pkt, data, len);
-        // TESTED
     case ETH_P_IPV6:
         return parse_ipv6(pkt, data, len);
-        // TESTED
     case ETH_P_PPP_SES:
         return parse_pppoe_ses(pkt, data, len);
-        // TESTED
     case ETH_P_MPLS_UC:
         return parse_mpls(pkt, data, len);
     default:
-        // TESTED ARP
         PACKET_LOG_UNSUPPORT_ETHPROTO("l3", next_proto);
         return data;
     }
@@ -1421,25 +1266,18 @@ static inline const char *parse_l4(struct packet *pkt, uint8_t next_proto, const
 {
     switch (next_proto)
     {
-        // TESTED
     case IPPROTO_TCP:
         return parse_tcp(pkt, data, len);
-        // TESTED
     case IPPROTO_UDP:
         return parse_udp(pkt, data, len);
-        // TESTED
     case IPPROTO_IPIP:
         return parse_ipv4(pkt, data, len);
-        // TESTED
     case IPPROTO_IPV6:
         return parse_ipv6(pkt, data, len);
-        // TESTED
     case IPPROTO_GRE:
         return parse_gre(pkt, data, len);
-        // TESTED
     case IPPROTO_ICMP:
         return parse_icmp(pkt, data, len);
-        // TESTED
     case IPPROTO_ICMPV6:
         return parse_icmp6(pkt, data, len);
     case 115:
@@ -1465,7 +1303,6 @@ const char *packet_parse(struct packet *pkt, const char *data, uint16_t len)
     pkt->data_len = len;
     pkt->trim_len = 0;
 
-    // TESTED
     return parse_ether(pkt, data, len);
 }
 
@@ -1490,6 +1327,9 @@ void packet_print_str(const struct packet *pkt)
         switch (layer->type)
         {
         case LAYER_TYPE_ETHER:
+            used = eth_hdr_to_str((const struct ethhdr *)layer->hdr_ptr, buffer, sizeof(buffer));
+            break;
+        case LAYER_TYPE_PWETH:
             break;
         case LAYER_TYPE_PPP:
             break;
@@ -1502,6 +1342,8 @@ void packet_print_str(const struct packet *pkt)
         case LAYER_TYPE_PPPOE:
             break;
         case LAYER_TYPE_MPLS:
+            used = mpls_hdr_to_str((const struct mpls_hdr *)layer->hdr_ptr, buffer, sizeof(buffer));
+            break;
             break;
         case LAYER_TYPE_IPV4:
             used = ipv4_hdr_to_str((const struct ip *)layer->hdr_ptr, buffer, sizeof(buffer));

src/packet/tcp_utils.h

@@ -262,6 +262,10 @@ static inline void tcp_hdr_set_opt_data(struct tcphdr *hdr, const char *ptr)
     memcpy((char *)hdr + sizeof(struct tcphdr), ptr, tcp_hdr_get_opt_len(hdr));
 }
 
+/******************************************************************************
+ * print
+ ******************************************************************************/
+
 static inline int tcp_hdr_to_str(const struct tcphdr *hdr, char *buf, size_t size)
 {
     memset(buf, 0, size);

src/packet/test/CMakeLists.txt

@@ -13,6 +13,12 @@ target_link_libraries(gtest_ipv4_utils packet gtest)
 add_executable(gtest_ipv6_utils gtest_ipv6_utils.cpp)
 target_link_libraries(gtest_ipv6_utils packet gtest)
 
+add_executable(gtest_mpls_utils gtest_mpls_utils.cpp)
+target_link_libraries(gtest_mpls_utils packet gtest)
+
+add_executable(gtest_eth_utils gtest_eth_utils.cpp)
+target_link_libraries(gtest_eth_utils packet gtest)
+
 add_executable(gtest_packet_frag gtest_packet_frag.cpp)
 target_link_libraries(gtest_packet_frag packet gtest)
 
@@ -22,4 +28,6 @@ gtest_discover_tests(gtest_udp_utils)
 gtest_discover_tests(gtest_tcp_utils)
 gtest_discover_tests(gtest_ipv4_utils)
 gtest_discover_tests(gtest_ipv6_utils)
+gtest_discover_tests(gtest_mpls_utils)
+gtest_discover_tests(gtest_eth_utils)
 gtest_discover_tests(gtest_packet_frag)

src/packet/test/gtest_eth_utils.cpp

@@ -0,0 +1,49 @@
+#include <gtest/gtest.h>
+
+#include "eth_utils.h"
+
+/*
+ * Ethernet II, Src: 00:00:00_00:04:36 (00:00:00:00:04:36), Dst: 18:10:04:00:03:1f (18:10:04:00:03:1f)
+ *     Destination: 18:10:04:00:03:1f (18:10:04:00:03:1f)
+ *         Address: 18:10:04:00:03:1f (18:10:04:00:03:1f)
+ *         .... ..0. .... .... .... .... = LG bit: Globally unique address (factory default)
+ *         .... ...0 .... .... .... .... = IG bit: Individual address (unicast)
+ *     Source: 00:00:00_00:04:36 (00:00:00:00:04:36)
+ *         Address: 00:00:00_00:04:36 (00:00:00:00:04:36)
+ *         .... ..0. .... .... .... .... = LG bit: Globally unique address (factory default)
+ *         .... ...0 .... .... .... .... = IG bit: Individual address (unicast)
+ *     Type: MPLS label switched packet (0x8847)
+ */
+
+unsigned char data[] = {
+    0x18, 0x10, 0x04, 0x00, 0x03, 0x1f, 0x00, 0x00, 0x00, 0x00, 0x04, 0x36, 0x88, 0x47};
+
+TEST(ETH_UTILS, GET)
+{
+    const struct ethhdr *hdr = (struct ethhdr *)data;
+
+    char dest[18] = {0};
+    char source[18] = {0};
+    EXPECT_TRUE(eth_hdr_get_dest(hdr, dest, sizeof(dest)) == 17);
+    EXPECT_TRUE(eth_hdr_get_source(hdr, source, sizeof(source)) == 17);
+    EXPECT_TRUE(memcmp(dest, "18:10:04:00:03:1f", 17) == 0);
+    EXPECT_TRUE(memcmp(source, "00:00:00:00:04:36", 17) == 0);
+    EXPECT_TRUE(eth_hdr_get_proto(hdr) == ETH_P_MPLS_UC);
+}
+
+TEST(ETH_UTILS, SET)
+{
+    char buff[14] = {0};
+    struct ethhdr *hdr = (struct ethhdr *)buff;
+
+    eth_hdr_set_dest(hdr, "18:10:04:00:03:1f");
+    eth_hdr_set_source(hdr, "00:00:00:00:04:36");
+    eth_hdr_set_proto(hdr, ETH_P_MPLS_UC);
+    EXPECT_TRUE(memcmp(buff, data, 14) == 0);
+}
+
+int main(int argc, char **argv)
+{
+    ::testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}

src/packet/test/gtest_mpls_utils.cpp

@@ -0,0 +1,42 @@
+#include <gtest/gtest.h>
+
+#include "mpls_utils.h"
+
+/*
+ * MultiProtocol Label Switching Header, Label: 779408, Exp: 0, S: 0, TTL: 255
+ *     1011 1110 0100 1001 0000 .... .... .... = MPLS Label: 779408 (0xbe490)
+ *     .... .... .... .... .... 000. .... .... = MPLS Experimental Bits: 0
+ *     .... .... .... .... .... ...0 .... .... = MPLS Bottom Of Label Stack: 0
+ *     .... .... .... .... .... .... 1111 1111 = MPLS TTL: 255
+ */
+
+unsigned char data[] = {
+    0xbe, 0x49, 0x00, 0xff};
+
+TEST(MPLS_UTILS, GET)
+{
+    const struct mpls_hdr *hdr = (struct mpls_hdr *)data;
+
+    EXPECT_TRUE(mpls_hdr_get_label(hdr) == 0xbe490);
+    EXPECT_TRUE(mpls_hdr_get_exp(hdr) == 0);
+    EXPECT_TRUE(mpls_hdr_get_bos(hdr) == 0);
+    EXPECT_TRUE(mpls_hdr_get_ttl(hdr) == 255);
+}
+
+TEST(MPLS_UTILS, SET)
+{
+    char buff[4] = {0};
+    struct mpls_hdr *hdr = (struct mpls_hdr *)buff;
+
+    mpls_hdr_set_label(hdr, 0xbe490);
+    mpls_hdr_set_exp(hdr, 0);
+    mpls_hdr_set_bos(hdr, 0);
+    mpls_hdr_set_ttl(hdr, 255);
+    EXPECT_TRUE(memcmp(buff, data, 4) == 0);
+}
+
+int main(int argc, char **argv)
+{
+    ::testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}

src/packet/test/gtest_packet.cpp

@@ -2628,8 +2628,8 @@ TEST(PACKET, ETH_MPLS_MPLS_PWETHCW_ETH_ARP)
 
 	EXPECT_TRUE(inner_mpls_record != nullptr);
 	EXPECT_TRUE(inner_mpls_record->hdr_offset == 18);
-	EXPECT_TRUE(inner_mpls_record->hdr_len == 4 + 4); // MPLS + PWETH
+	EXPECT_TRUE(inner_mpls_record->hdr_len == 4);
-	EXPECT_TRUE(inner_mpls_record->pld_len == 64);
+	EXPECT_TRUE(inner_mpls_record->pld_len == 68);
 
 	// LAYER_TYPE_L2_TUN
 	const struct packet_layer *inner_l2_tun_record = packet_get_innermost_layer(&handler, LAYER_TYPE_L2_TUN);
@@ -2637,6 +2637,14 @@ TEST(PACKET, ETH_MPLS_MPLS_PWETHCW_ETH_ARP)
 	EXPECT_TRUE(inner_l2_tun_record != nullptr);
 	EXPECT_TRUE(inner_l2_tun_record == inner_mpls_record);
 
+	// LAYER_TYPE_PWETHCW
+	const struct packet_layer *inner_pweth_record = packet_get_innermost_layer(&handler, LAYER_TYPE_PWETH);
+
+	EXPECT_TRUE(inner_pweth_record != nullptr);
+	EXPECT_TRUE(inner_pweth_record->hdr_offset == 22);
+	EXPECT_TRUE(inner_pweth_record->hdr_len == 4);
+	EXPECT_TRUE(inner_pweth_record->pld_len == 64);
+
 	// LAYER_TYPE_ETHER
 	const struct packet_layer *inner_eth_record = packet_get_innermost_layer(&handler, LAYER_TYPE_ETHER);
 

src/packet/udp_utils.h

@@ -7,6 +7,7 @@ extern "C"
 #endif
 
 #include <stdio.h>
+#include <string.h>
 #include <arpa/inet.h>
 #define __FAVOR_BSD 1
 #include <netinet/udp.h>
@@ -73,6 +74,10 @@ static inline void udp_hdr_set_checksum(struct udphdr *hdr, uint16_t sum)
     hdr->uh_sum = htons(sum);
 }
 
+/******************************************************************************
+ * print
+ ******************************************************************************/
+
 static inline int udp_hdr_to_str(const struct udphdr *hdr, char *buf, size_t size)
 {
     memset(buf, 0, size);

test/packet_parser/packet_parser.cpp

@@ -52,6 +52,9 @@ static int packet_proto_to_str(const struct packet *pkt, char *buff, int size)
         case LAYER_TYPE_ETHER:
             used += snprintf(buff + used, size - used, "eth:ethertype");
             break;
+        case LAYER_TYPE_PWETH:
+            used += snprintf(buff + used, size - used, "pweth:ethertype");
+            break;
         case LAYER_TYPE_PPP:
             used += snprintf(buff + used, size - used, "ppp");
             break;