diff options
Diffstat (limited to 'common/src/packet_parser.cpp')
| -rw-r--r-- | common/src/packet_parser.cpp | 668 |
1 files changed, 668 insertions, 0 deletions
diff --git a/common/src/packet_parser.cpp b/common/src/packet_parser.cpp new file mode 100644 index 0000000..6e2e2c3 --- /dev/null +++ b/common/src/packet_parser.cpp @@ -0,0 +1,668 @@ +#include <string.h> +#include <netinet/ip.h> +#include <netinet/ip6.h> +#define __FAVOR_BSD 1 +#include <netinet/tcp.h> +#include <netinet/ether.h> + +#include "log.h" +#include "packet_parser.h" + +#define LOG_PKT_PARSER "PACKET_PARSER" + +/****************************************************************************** + * Struct + ******************************************************************************/ + +struct udp_hdr +{ + uint16_t uh_sport; /* source port */ + uint16_t uh_dport; /* destination port */ + uint16_t uh_ulen; /* udp length */ + uint16_t uh_sum; /* udp checksum */ +} __attribute__((__packed__)); + +struct vlan_hdr +{ + uint16_t vlan_cfi; + uint16_t protocol; +} __attribute__((__packed__)); + +struct vxlan_hdr +{ + uint8_t flags[2]; + uint16_t gdp; // group policy id + uint8_t vni[3]; + uint8_t reserved; +} __attribute__((__packed__)); + +struct gtp_hdr +{ +#if __BYTE_ORDER == __LITTLE_ENDIAN + unsigned char flags; + unsigned char msg_type; + unsigned short len; + unsigned int teid; +#elif __BYTE_ORDER == __BIG_ENDIAN + unsigned int teid; + unsigned short len; + unsigned char msg_type; + unsigned char flags; +#else +#error "Please check <endian.h>" +#endif +} __attribute__((__packed__)); + +#define GTP_HDR_VER_MASK (0xE0) +#define GTP_HDR_FLAG_N_PDU (0x01) +#define GTP_HDR_FLAG_SEQ_NUM (0x02) +#define GTP_HDR_FLAG_NEXT_EXT_HDR (0x04) + +/****************************************************************************** + * Static API + ******************************************************************************/ + +static int packet_parser_push(struct packet_parser *handler, enum layer_type type, uint16_t hdr_offset, uint16_t hdr_len, uint16_t payload_len); + +// parser utils +static const char *layer2str(enum layer_type type); +static uint16_t parse_gtphdr_len(const struct gtp_hdr *gtph); + +// parser protocol +static const void *parse_ether(struct packet_parser *handler, const void *data, uint16_t length); +static const void *parse_ipv4(struct packet_parser *handler, const void *data, uint16_t length); +static const void *parse_ipv6(struct packet_parser *handler, const void *data, uint16_t length); +static const void *parse_tcp(struct packet_parser *handler, const void *data, uint16_t length); +static const void *parse_udp(struct packet_parser *handler, const void *data, uint16_t length); +static const void *parse_pppoe_ses(struct packet_parser *handler, const void *data, uint16_t length); +static const void *parse_vxlan(struct packet_parser *handler, const void *data, uint16_t length); +static const void *parse_vlan8021q(struct packet_parser *handler, const void *data, uint16_t length); +static const void *parse_gtpv1_u(struct packet_parser *handler, const void *data, uint16_t length); +static const void *parse_mpls(struct packet_parser *handler, const void *data, uint16_t length); + +/****************************************************************************** + * Public API + ******************************************************************************/ + +void packet_parser_init(struct packet_parser *handler) +{ + memset(handler, 0, sizeof(struct packet_parser)); + + handler->result.used = 0; + handler->result.size = sizeof(handler->result.layers) / sizeof(handler->result.layers[0]); + handler->packet_data = NULL; + handler->packet_len = 0; + handler->packet_id = 0; +} + +// return most inner payload +const void *packet_parser_parse(struct packet_parser *handler, const void *packet_data, uint16_t packet_len, uint64_t packet_id) +{ + handler->packet_data = packet_data; + handler->packet_len = packet_len; + handler->packet_id = packet_id; + + // TESTED OK BY LWP + return parse_ether(handler, handler->packet_data, handler->packet_len); +} + +const struct layer_record *packet_parser_get_most_inner(struct packet_parser *handler, enum layer_type type) +{ + struct parser_result *result = &handler->result; + for (int16_t i = result->used - 1; i >= 0; i--) + { + const struct layer_record *layer = &result->layers[i]; + if (layer->type & type) + { + return layer; + } + } + + return NULL; +} + +const struct layer_record *packet_parser_get_most_outer(struct packet_parser *handler, enum layer_type type) +{ + struct parser_result *result = &handler->result; + for (int16_t i = 0; i <= result->used - 1; i++) + { + const struct layer_record *layer = &result->layers[i]; + if (layer->type & type) + { + return layer; + } + } + + return NULL; +} + +// return 4: IPv4 +// return 6: IPv6 +uint8_t gtp_next_proto(const char *data) +{ + uint16_t hdr_len = parse_gtphdr_len((const struct gtp_hdr *)data); + if (hdr_len < 0) + { + return 0; + } + + uint8_t next_proto = (((const uint8_t *)(data + hdr_len))[0]) >> 4; + return next_proto; +} + +/****************************************************************************** + * Private API + ******************************************************************************/ + +static int packet_parser_push(struct packet_parser *handler, enum layer_type type, uint16_t hdr_offset, uint16_t hdr_len, uint16_t payload_len) +{ + struct parser_result *result = &handler->result; + if (result->used >= result->size) + { + return -1; + } + + result->layers[result->used].type = type; + result->layers[result->used].hdr_offset = hdr_offset; + result->layers[result->used].hdr_len = hdr_len; + result->layers[result->used].pld_len = payload_len; + result->used++; + + return 0; +} + +static const char *layer2str(enum layer_type type) +{ + switch (type) + { + case LAYER_TYPE_ETHER: + return "ETHER"; + case LAYER_TYPE_PPP: + return "PPP"; + case LAYER_TYPE_HDLC: + return "HDLC"; + case LAYER_TYPE_VLAN: + return "VLAN"; + case LAYER_TYPE_PPPOE: + return "PPPOE"; + case LAYER_TYPE_MPLS: + return "MPLS"; + case LAYER_TYPE_IPV4: + return "IPV4"; + case LAYER_TYPE_IPV6: + return "IPV6"; + case LAYER_TYPE_UDP: + return "UDP"; + case LAYER_TYPE_TCP: + return "TCP"; + case LAYER_TYPE_G_VXLAN: + return "G_VXLAN"; + case LAYER_TYPE_GTPV1_U: + return "GTPV1_U"; + default: + return "UNKNOWN"; + } +} + +// FROM SAPP +static uint16_t parse_gtphdr_len(const struct gtp_hdr *gtph) +{ + const unsigned char *p_ext_hdr = (unsigned char *)gtph + sizeof(struct gtp_hdr); + unsigned char next_hdr_type; + unsigned char this_ext_field_cont_len; + + // v0 太古老已废弃，目前仅支持 GTPv1 版本 + if (((gtph->flags & GTP_HDR_VER_MASK) >> 5) != 1) + { + return -1; + } + + if (gtph->flags & (GTP_HDR_FLAG_SEQ_NUM | GTP_HDR_FLAG_N_PDU | GTP_HDR_FLAG_NEXT_EXT_HDR)) + { + // skip seq field (2 bytes) + p_ext_hdr += 2; + + // skip N-PDU field (1 byte) + p_ext_hdr++; + + // 解析 GTP 扩展头部字段，参考 wireshark 源码 packet-gtp.c->dissect_gtp_common() + next_hdr_type = *p_ext_hdr; + if (gtph->flags & GTP_HDR_FLAG_NEXT_EXT_HDR) + { + while (next_hdr_type != 0) + { + // 指向长度字段, 以4个字节为单位 + p_ext_hdr++; + this_ext_field_cont_len = *p_ext_hdr * 4 - 2; + + // 指向数据部分第一个字节 + p_ext_hdr++; + p_ext_hdr += this_ext_field_cont_len; + + // 指向下一个头部字段 + next_hdr_type = *p_ext_hdr; + p_ext_hdr++; + } + } + else + { + p_ext_hdr++; + } + } + + return (char *)p_ext_hdr - (char *)gtph; +} + +static const void *parse_ether(struct packet_parser *handler, const void *data, uint16_t length) +{ + if (length < sizeof(struct ethhdr)) + { + LOG_ERROR("%s: trace id: %lu, layer: %s, err: data not enough", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_ETHER)); + return data; + } + + struct ethhdr *hdr = (struct ethhdr *)data; + uint16_t next_proto = ntohs(hdr->h_proto); + uint16_t hdr_len = sizeof(struct ethhdr); + uint16_t hdr_offset = (uintptr_t)data - (uintptr_t)(handler->packet_data); + uint16_t payload_len = length - hdr_len; + const void *payload_data = (const char *)data + hdr_len; + + if (packet_parser_push(handler, LAYER_TYPE_ETHER, hdr_offset, hdr_len, payload_len) != 0) + { + return data; + } + + LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_ETHER), payload_len, length); + switch (next_proto) + { + case ETH_P_8021Q: + // TESTED OK BY LWP + return parse_vlan8021q(handler, payload_data, payload_len); + case ETH_P_8021AD: + return parse_ether(handler, payload_data, payload_len); + case ETH_P_IP: + // TESTED OK BY LWP + return parse_ipv4(handler, payload_data, payload_len); + case ETH_P_IPV6: + // TESTED OK BY LWP + return parse_ipv6(handler, payload_data, payload_len); + case ETH_P_PPP_SES: + return parse_pppoe_ses(handler, payload_data, payload_len); + case ETH_P_MPLS_UC: + // TESTED OK BY LWP + return parse_mpls(handler, payload_data, payload_len); + default: + LOG_ERROR("%s: trace id: %lu, layer: %s, stop parse next protocol %d", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_ETHER), next_proto); + return payload_data; + } +} + +static const void *parse_ipv4(struct packet_parser *handler, const void *data, uint16_t length) +{ + if (length < sizeof(struct ip)) + { + LOG_ERROR("%s: trace id: %lu, layer: %s, err: data not enough", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_IPV4)); + return data; + } + + struct ip *hdr = (struct ip *)data; + uint16_t next_proto = hdr->ip_p; + uint16_t hdr_len = (hdr->ip_hl & 0xf) * 4u; + uint16_t hdr_offset = (uintptr_t)data - (uintptr_t)(handler->packet_data); + uint16_t payload_len = length - hdr_len; + const void *payload_data = (const char *)data + hdr_len; + + if (packet_parser_push(handler, LAYER_TYPE_IPV4, hdr_offset, hdr_len, payload_len) != 0) + { + return data; + } + + LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_IPV4), payload_len, length); + switch (next_proto) + { + case IPPROTO_TCP: + // TESTED OK BY LWP + return parse_tcp(handler, payload_data, payload_len); + case IPPROTO_UDP: + // TESTED OK BY LWP + return parse_udp(handler, payload_data, payload_len); + case IPPROTO_IPIP: + // TESTED OK BY LWP + return parse_ipv4(handler, payload_data, payload_len); + case IPPROTO_IPV6: + // TESTED OK BY LWP + return parse_ipv6(handler, payload_data, payload_len); + default: + // TODO GRE + LOG_ERROR("%s: trace id: %lu, layer: %s, stop parse next protocol %d", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_IPV4), next_proto); + return payload_data; + } +} + +static const void *parse_ipv6(struct packet_parser *handler, const void *data, uint16_t length) +{ + if (length < sizeof(struct ip6_hdr)) + { + LOG_ERROR("%s: trace id: %lu, layer: %s, err: data not enough", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_IPV6)); + return data; + } + + struct ip6_hdr *hdr = (struct ip6_hdr *)data; + uint16_t next_proto = hdr->ip6_nxt; + uint16_t hdr_len = sizeof(struct ip6_hdr); + uint16_t hdr_offset = (uintptr_t)data - (uintptr_t)(handler->packet_data); + uint16_t payload_len = length - hdr_len; + const void *payload_data = (const char *)data + hdr_len; + + if (packet_parser_push(handler, LAYER_TYPE_IPV6, hdr_offset, hdr_len, payload_len) != 0) + { + return data; + } + + LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_IPV6), payload_len, length); + switch (next_proto) + { + case IPPROTO_TCP: + // TESTED OK BY LWP + return parse_tcp(handler, payload_data, payload_len); + case IPPROTO_UDP: + // TESTED OK BY LWP + return parse_udp(handler, payload_data, payload_len); + case IPPROTO_IPIP: + // TESTED OK BY LWP + return parse_ipv4(handler, payload_data, payload_len); + case IPPROTO_IPV6: + // TESTED OK BY LWP + return parse_ipv6(handler, payload_data, payload_len); + default: + LOG_ERROR("%s: trace id: %lu, layer: %s, stop parse next protocol %d", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_IPV6), next_proto); + return payload_data; + } +} + +static const void *parse_tcp(struct packet_parser *handler, const void *data, uint16_t length) +{ + if (length < sizeof(struct tcphdr)) + { + LOG_ERROR("%s: trace id: %lu, layer: %s, err: data not enough", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_TCP)); + return data; + } + + struct tcphdr *hdr = (struct tcphdr *)data; + uint16_t hdr_len = hdr->th_off << 2; + uint16_t hdr_offset = (uintptr_t)data - (uintptr_t)(handler->packet_data); + uint16_t payload_len = length - hdr_len; + const void *payload_data = (const char *)data + hdr_len; + + if (packet_parser_push(handler, LAYER_TYPE_TCP, hdr_offset, hdr_len, payload_len) != 0) + { + return data; + } + + LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_TCP), payload_len, length); + + // TESTED OK BY LWP + return payload_data; +} + +static const void *parse_udp(struct packet_parser *handler, const void *data, uint16_t length) +{ + if (length < sizeof(struct udp_hdr)) + { + LOG_ERROR("%s: trace id: %lu, layer: %s, err: data not enough", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_UDP)); + return data; + } + + struct udp_hdr *hdr = (struct udp_hdr *)data; + uint16_t hdr_len = sizeof(struct udp_hdr); + uint16_t hdr_offset = (uintptr_t)data - (uintptr_t)(handler->packet_data); + uint16_t payload_len = length - hdr_len; + const void *payload_data = (const char *)data + hdr_len; + + if (packet_parser_push(handler, LAYER_TYPE_UDP, hdr_offset, hdr_len, payload_len) != 0) + { + return data; + } + + LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_UDP), payload_len, length); + switch (ntohs(hdr->uh_dport)) + { + case 4789: // VXLAN_DPORT + // TESTED OK BY LWP + return parse_vxlan(handler, payload_data, payload_len); + case 2152: // GTP1U_PORT + // TESTED OK BY LWP + return parse_gtpv1_u(handler, payload_data, payload_len); + default: + // TESTED OK BY LWP + return payload_data; + } +} + +static const void *parse_pppoe_ses(struct packet_parser *handler, const void *data, uint16_t length) +{ + if (length < 8) + { + LOG_ERROR("%s: trace id: %lu, layer: %s, err: data not enough", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_PPPOE)); + return data; + } + + uint16_t next_proto = *((uint16_t *)data + 3); + uint16_t hdr_len = 8; + uint16_t hdr_offset = (uintptr_t)data - (uintptr_t)(handler->packet_data); + uint16_t payload_len = length - hdr_len; + const void *payload_data = (const char *)data + hdr_len; + + if (packet_parser_push(handler, LAYER_TYPE_PPPOE, hdr_offset, hdr_len, payload_len) != 0) + { + return data; + } + + LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_PPPOE), payload_len, length); + switch (next_proto) + { + case 0x2100: // PPPOE_TYPE_IPV4 + // TESTED OK BY LWP + return parse_ipv4(handler, payload_data, payload_len); + case 0x5700: // PPPOE_TYPE_IPV6 + return parse_ipv6(handler, payload_data, payload_len); + default: + LOG_ERROR("%s: trace id: %lu, layer: %s, stop parse next protocol %d", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_PPPOE), next_proto); + return payload_data; + } +} + +static const void *parse_vxlan(struct packet_parser *handler, const void *data, uint16_t length) +{ + if (length < sizeof(struct vxlan_hdr)) + { + LOG_ERROR("%s: trace id: %lu, layer: %s, err: data not enough", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_G_VXLAN)); + return NULL; + } + + // struct vxlan_hdr *vxlan_hdr = (struct vxlan_hdr *)data; + uint16_t hdr_len = sizeof(struct vxlan_hdr); + uint16_t hdr_offset = (uintptr_t)data - (uintptr_t)(handler->packet_data); + uint16_t payload_len = length - hdr_len; + const void *payload_data = (const char *)data + hdr_len; + + if (packet_parser_push(handler, LAYER_TYPE_G_VXLAN, hdr_offset, hdr_len, payload_len) != 0) + { + return data; + } + + LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_G_VXLAN), payload_len, length); + // TESTED OK BY LWP + return parse_ether(handler, payload_data, payload_len); +} + +static const void *parse_vlan8021q(struct packet_parser *handler, const void *data, uint16_t length) +{ + if (length < sizeof(struct vlan_hdr)) + { + LOG_ERROR("%s: trace id: %lu, layer: %s, err: data not enough", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_VLAN)); + return NULL; + } + + struct vlan_hdr *hdr = (struct vlan_hdr *)data; + uint16_t next_proto = ntohs(hdr->protocol); + uint16_t hdr_len = sizeof(struct vlan_hdr); + uint16_t hdr_offset = (uintptr_t)data - (uintptr_t)(handler->packet_data); + uint16_t payload_len = length - hdr_len; + const void *payload_data = (const char *)data + hdr_len; + + if (packet_parser_push(handler, LAYER_TYPE_VLAN, hdr_offset, hdr_len, payload_len) != 0) + { + return data; + } + + LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_VLAN), payload_len, length); + switch (next_proto) + { + case ETH_P_8021Q: + // TESTED OK BY LWP + return parse_vlan8021q(handler, payload_data, payload_len); + case ETH_P_IP: + // TESTED OK BY LWP + return parse_ipv4(handler, payload_data, payload_len); + case ETH_P_IPV6: + // TESTED OK BY LWP + return parse_ipv6(handler, payload_data, payload_len); + case ETH_P_PPP_SES: + // TESTED OK BY LWP + return parse_pppoe_ses(handler, payload_data, payload_len); + case ETH_P_MPLS_UC: + return parse_mpls(handler, payload_data, payload_len); + default: + LOG_ERROR("%s: trace id: %lu, layer: %s, stop parse next protocol %d", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_VLAN), next_proto); + return payload_data; + } +} + +static const void *parse_gtpv1_u(struct packet_parser *handler, const void *data, uint16_t length) +{ + if (length < sizeof(struct gtp_hdr)) + { + LOG_ERROR("%s: trace id: %lu, layer: %s, err: data not enough", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_GTPV1_U)); + return NULL; + } + + uint16_t hdr_len = parse_gtphdr_len((const struct gtp_hdr *)data); + if (hdr_len < 0) + { + return data; + } + + uint8_t next_proto = (((const uint8_t *)((const char *)data + hdr_len))[0]) >> 4; + uint16_t hdr_offset = (uintptr_t)data - (uintptr_t)(handler->packet_data); + uint16_t payload_len = length - hdr_len; + const void *payload_data = (const char *)data + hdr_len; + + if (packet_parser_push(handler, LAYER_TYPE_GTPV1_U, hdr_offset, hdr_len, payload_len) != 0) + { + return data; + } + + LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_GTPV1_U), payload_len, length); + switch (next_proto) + { + case 4: + // TESTED OK BY LWP + return parse_ipv4(handler, payload_data, payload_len); + case 6: + // TESTED OK BY LWP + return parse_ipv6(handler, payload_data, payload_len); + default: + LOG_ERROR("%s: trace id: %lu, layer: %s, stop parse next protocol %d", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_GTPV1_U), next_proto); + return payload_data; + } +} + +static const void *parse_mpls(struct packet_parser *handler, const void *data, uint16_t length) +{ + if (length < 4) + { + LOG_ERROR("%s: trace id: %lu, layer: %s, err: data not enough", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_MPLS)); + return data; + } + +#define MPLS_LABEL_MASK (0xFFFFF000) +#define MPLS_EXP_MASK (0x00000E00) +#define MPLS_BLS_MASK (0x00000100) +#define MPLS_TTL_MASK (0x000000FF) + + /* + * 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 + */ + + uint32_t *hdr = (uint32_t *)data; + // unsigned int mpls_label = (ntohl(*hdr) & MPLS_LABEL_MASK) >> 12; + // unsigned int mpls_exp = (ntohl(*hdr) & MPLS_EXP_MASK) >> 9; + unsigned int mpls_bls = (ntohl(*hdr) & MPLS_BLS_MASK) >> 8; + // unsigned int mpls_ttl = (ntohl(*hdr) & MPLS_TTL_MASK); + + uint16_t hdr_len = 4; + uint16_t hdr_offset = (uintptr_t)data - (uintptr_t)(handler->packet_data); + uint16_t payload_len = length - hdr_len; + const void *payload_data = (const char *)data + hdr_len; + + if (packet_parser_push(handler, LAYER_TYPE_MPLS, hdr_offset, hdr_len, payload_len) != 0) + { + return data; + } + + if (mpls_bls == 1) + { + uint8_t ip_version = (((uint8_t *)payload_data)[0]) >> 4; + if (ip_version == 0) + { + /* + * 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 + */ + payload_data = (const char *)payload_data + 4; + payload_len = payload_len - 4; + + LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_MPLS), payload_len, length); + return parse_ether(handler, payload_data, payload_len); + } + else if (ip_version == 4) + { + LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_MPLS), payload_len, length); + // TESTED OK BY LWP + return parse_ipv4(handler, payload_data, payload_len); + } + else if (ip_version == 6) + { + LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_MPLS), payload_len, length); + return parse_ipv6(handler, payload_data, payload_len); + } + else + { + LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_MPLS), payload_len, length); + // TESTED OK BY LWP + return parse_ether(handler, payload_data, payload_len); + } + } + else + { + LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_MPLS), payload_len, length); + // TESTED OK BY LWP + return parse_mpls(handler, payload_data, payload_len); + } +}
\ No newline at end of file |