diff options
Diffstat (limited to 'common/src/packet.cpp')
| -rw-r--r-- | common/src/packet.cpp | 1475 |
1 files changed, 1475 insertions, 0 deletions
diff --git a/common/src/packet.cpp b/common/src/packet.cpp new file mode 100644 index 0000000..b96edcc --- /dev/null +++ b/common/src/packet.cpp @@ -0,0 +1,1475 @@ +#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 <linux/ppp_defs.h> + +#include "uthash.h" +#include "packet.h" + +#define likely(expr) __builtin_expect((expr), 1) +#define unlikely(expr) __builtin_expect((expr), 0) + +#define PACKET_LOG_DATA_INSUFFICIENCY(type) \ + { \ + PACKET_LOG_ERROR("layer: %s, data insufficiency", \ + layer_type_tostring((type))); \ + } + +#define PACKET_LOG_UNSUPPORT_PROTO(tag, next_proto) \ + { \ + PACKET_LOG_ERROR("%s: unsupport next proto %d", \ + (tag), (next_proto)); \ + } + +#define PACKET_LOG_UNSUPPORT_ETHPROTO(tag, next_proto) \ + { \ + PACKET_LOG_ERROR("%s: unsupport next proto %d: %s", \ + (tag), (next_proto), ethproto_tostring(next_proto)); \ + } + +#define PACKET_LOG_UNSUPPORT_IPPROTO(tag, next_proto) \ + { \ + PACKET_LOG_ERROR("%s: unsupport next proto %d: %s", \ + (tag), (next_proto), ipproto_tostring(next_proto)); \ + } + +/****************************************************************************** + * Static API + ******************************************************************************/ + +static const char *ethproto_tostring(uint16_t proto); +static const char *ipproto_tostring(uint16_t proto); +static inline const char *ldbc_method_tostring(enum ldbc_method method); +static inline const char *layer_type_tostring(enum layer_type type); + +static inline void set_tuple2(const char *data, enum layer_type type, struct tuple2 *tuple); +static inline void set_tuple4(const char *data, enum layer_type type, struct tuple4 *tuple); +static inline void set_tuple6(const char *data, enum layer_type type, struct tuple6 *tuple, uint64_t zone_id); + +static inline struct layer_record *get_free_layer(struct packet *handler); + +static inline uint16_t get_gtp_hdr_len(const char *data, uint16_t len); +static inline uint16_t get_gre_hdr_len(const char *data, uint16_t len); + +// 数据链路层 +static inline const char *parse_ether(struct packet *handler, const char *data, uint16_t len); +static inline const char *parse_ppp(struct packet *handler, const char *data, uint16_t len); +// 数据链路层 -- 隧道 +static inline const char *parse_vlan(struct packet *handler, const char *data, uint16_t len); +static inline const char *parse_pppoe_ses(struct packet *handler, const char *data, uint16_t len); +static inline const char *parse_mpls(struct packet *handler, const char *data, uint16_t len); +// 网络层 +static inline const char *parse_ipv4(struct packet *handler, const char *data, uint16_t len); +static inline const char *parse_ipv6(struct packet *handler, const char *data, uint16_t len); +// 网络层 -- 隧道 +static inline const char *parse_gre(struct packet *handler, const char *data, uint16_t len); +// 传输层 +static inline const char *parse_udp(struct packet *handler, const char *data, uint16_t len); +static inline const char *parse_tcp(struct packet *handler, const char *data, uint16_t len); +// 传输层 -- 隧道 +static inline const char *parse_vxlan(struct packet *handler, const char *data, uint16_t len); +static inline const char *parse_gtpv1_u(struct packet *handler, const char *data, uint16_t len); +// L3/L4 +static inline const char *parse_l3(struct packet *handler, uint16_t next_proto, const char *data, uint16_t len); +static inline const char *parse_l4(struct packet *handler, uint8_t next_proto, const char *data, uint16_t len); + +/****************************************************************************** + * Private API -- Utils + ******************************************************************************/ + +static const char *ethproto_tostring(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_tostring(uint16_t proto) +{ + switch (proto) + { + case IPPROTO_IP: + return "IP"; + case IPPROTO_ICMP: + return "ICMP"; + case IPPROTO_IGMP: + return "IGMP"; + case IPPROTO_IPIP: + return "IPIP"; + case IPPROTO_TCP: + return "TCP"; + case IPPROTO_EGP: + return "EGP"; + case IPPROTO_PUP: + return "PUP"; + case IPPROTO_UDP: + return "UDP"; + case IPPROTO_IDP: + return "IDP"; + case IPPROTO_TP: + return "TP"; + case IPPROTO_DCCP: + return "DCCP"; + case IPPROTO_IPV6: + return "IPV6"; + case IPPROTO_ROUTING: + return "ROUTING"; + case IPPROTO_FRAGMENT: + return "FRAGMENT"; + case IPPROTO_RSVP: + return "RSVP"; + case IPPROTO_GRE: + return "GRE"; + case IPPROTO_ESP: + return "ESP"; + case IPPROTO_AH: + return "AH"; + case IPPROTO_ICMPV6: + return "ICMPV6"; + case IPPROTO_NONE: + return "NONE"; + case IPPROTO_DSTOPTS: + return "DSTOPTS"; + case IPPROTO_MTP: + return "MTP"; + case IPPROTO_ENCAP: + return "ENCAP"; + case IPPROTO_PIM: + return "PIM"; + case IPPROTO_COMP: + return "COMP"; + case IPPROTO_SCTP: + return "SCTP"; + case IPPROTO_UDPLITE: + return "UDPLITE"; + case IPPROTO_RAW: + return "RAW"; + default: + return "UNKNOWN"; + } +} + +static inline const char *ldbc_method_tostring(enum ldbc_method method) +{ + switch (method) + { + case LDBC_METHOD_HASH_INT_IP: + return "outter_internal_ip"; + case LDBC_METHOD_HASH_EXT_IP: + return "outter_external_ip"; + case LDBC_METHOD_HASH_INT_IP_AND_EXT_IP: + return "outter_internal_ip_and_external_ip"; + case LDBC_METHOD_HASH_INNERMOST_INT_IP: + return "inner_internal_ip"; + case LDBC_METHOD_HASH_INNERMOST_EXT_IP: + return "inner_external_ip"; + default: + return "unknown"; + } +} + +static inline const char *layer_type_tostring(enum layer_type type) +{ + switch (type) + { + case LAYER_TYPE_ETHER: + return "ETH"; + 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_GRE: + return "GRE"; + case LAYER_TYPE_UDP: + return "UDP"; + case LAYER_TYPE_TCP: + return "TCP"; + case LAYER_TYPE_VXLAN: + return "VXLAN"; + case LAYER_TYPE_GTPV1_U: + return "GTPV1"; + default: + return "UNKNOWN"; + } +} + +static inline void set_tuple2(const char *data, enum layer_type type, struct tuple2 *tuple) +{ + const struct ip *ipv4 = NULL; + const struct ip6_hdr *ipv6 = NULL; + + switch (type) + { + case LAYER_TYPE_IPV4: + ipv4 = (const struct ip *)data; + tuple->ip_type = IP_TYPE_V4; + tuple->src_addr.v4.s_addr = ipv4->ip_src.s_addr; + tuple->dst_addr.v4.s_addr = ipv4->ip_dst.s_addr; + break; + case LAYER_TYPE_IPV6: + ipv6 = (const struct ip6_hdr *)data; + tuple->ip_type = IP_TYPE_V6; + tuple->src_addr.v6 = ipv6->ip6_src; + tuple->dst_addr.v6 = ipv6->ip6_dst; + break; + default: + break; + } +} + +static inline void set_tuple4(const char *data, enum layer_type type, struct tuple4 *tuple) +{ + const struct ip *ipv4 = NULL; + const struct ip6_hdr *ipv6 = NULL; + const struct tcphdr *tcp = NULL; + const struct udphdr *udp = NULL; + + switch (type) + { + case LAYER_TYPE_TCP: + tcp = (const struct tcphdr *)data; + tuple->src_port = tcp->th_sport; + tuple->dst_port = tcp->th_dport; + break; + case LAYER_TYPE_UDP: + udp = (const struct udphdr *)data; + tuple->src_port = udp->uh_sport; + tuple->dst_port = udp->uh_dport; + break; + case LAYER_TYPE_IPV4: + ipv4 = (const struct ip *)data; + tuple->ip_type = IP_TYPE_V4; + tuple->src_addr.v4.s_addr = ipv4->ip_src.s_addr; + tuple->dst_addr.v4.s_addr = ipv4->ip_dst.s_addr; + break; + case LAYER_TYPE_IPV6: + ipv6 = (const struct ip6_hdr *)data; + tuple->ip_type = IP_TYPE_V6; + tuple->src_addr.v6 = ipv6->ip6_src; + tuple->dst_addr.v6 = ipv6->ip6_dst; + break; + default: + break; + } +} + +static inline void set_tuple6(const char *data, enum layer_type type, struct tuple6 *tuple, uint64_t zone_id) +{ + const struct ip *ipv4 = NULL; + const struct ip6_hdr *ipv6 = NULL; + const struct tcphdr *tcp = NULL; + const struct udphdr *udp = NULL; + + tuple->security_zone = zone_id; + + switch (type) + { + case LAYER_TYPE_TCP: + tcp = (const struct tcphdr *)data; + tuple->ip_proto = IPPROTO_TCP; + tuple->src_port = tcp->th_sport; + tuple->dst_port = tcp->th_dport; + break; + case LAYER_TYPE_UDP: + udp = (const struct udphdr *)data; + tuple->ip_proto = IPPROTO_UDP; + tuple->src_port = udp->uh_sport; + tuple->dst_port = udp->uh_dport; + break; + case LAYER_TYPE_IPV4: + ipv4 = (const struct ip *)data; + tuple->ip_type = IP_TYPE_V4; + tuple->src_addr.v4.s_addr = ipv4->ip_src.s_addr; + tuple->dst_addr.v4.s_addr = ipv4->ip_dst.s_addr; + break; + case LAYER_TYPE_IPV6: + ipv6 = (const struct ip6_hdr *)data; + tuple->ip_type = IP_TYPE_V6; + tuple->src_addr.v6 = ipv6->ip6_src; + tuple->dst_addr.v6 = ipv6->ip6_dst; + break; + default: + break; + } +} + +static inline struct layer_record *get_free_layer(struct packet *handler) +{ + if (handler->layers_used >= handler->layers_size) + { + return NULL; + } + + return &handler->layers[handler->layers_used]; +} + +#define SET_LAYER(_handler, _layer, _type, _hdr_len, _data, _len) \ + { \ + (_layer)->type = (_type); \ + (_layer)->hdr_offset = (_handler)->data_len - (_len); \ + (_layer)->hdr_ptr = (_data); \ + (_layer)->hdr_len = (_hdr_len); \ + (_layer)->pld_ptr = (_data) + (_hdr_len); \ + (_layer)->pld_len = (_len) - (_hdr_len); \ + (_handler)->layers_used++; \ + PACKET_LOG_DEBUG("layer[%d/%d]: %s, hdr_offset: %d, hdr_ptr: %p, hdr_len: %d, pld_ptr: %p, pld_len: %d", \ + (_handler)->layers_used - 1, (_handler)->layers_size, layer_type_tostring((_type)), \ + (_layer)->hdr_offset, (_layer)->hdr_ptr, (_layer)->hdr_len, (_layer)->pld_ptr, (_layer)->pld_len); \ + } + +/****************************************************************************** + * Private API -- Parses + ******************************************************************************/ + +static inline uint16_t get_gtp_hdr_len(const char *data, uint16_t len) +{ +#define GTP_HDR_VER (0xE0) +#define GTP_HDR_FLAG_N_PDU (0x01) +#define GTP_HDR_FLAG_SEQ_NUM (0x02) +#define GTP_HDR_FLAG_EXT_HDR (0x04) + + struct gtp_hdr + { + uint8_t flags; + uint8_t msg_type; + uint16_t msg_len; + uint32_t teid; + } __attribute__((__packed__)); + + struct gtp_opt + { + uint16_t seq_num; + uint8_t npdu; + uint8_t next_ext_hdr; + } __attribute__((__packed__)); + + uint16_t hdr_offset = 0; + if (len < sizeof(struct gtp_hdr)) + { + return 0; + } + const struct gtp_hdr *gtp = (const struct gtp_hdr *)data; + hdr_offset += sizeof(struct gtp_hdr); // skip gre hdr + + // GTPv0 Not Supported + if (((gtp->flags & GTP_HDR_VER) >> 5) != 1) + { + return 0; + } + + if (gtp->flags & (GTP_HDR_FLAG_SEQ_NUM | GTP_HDR_FLAG_N_PDU | GTP_HDR_FLAG_EXT_HDR)) + { + if (hdr_offset + sizeof(struct gtp_opt) > len) + { + return 0; + } + struct gtp_opt *opt_hdr = (struct gtp_opt *)((char *)data + hdr_offset); + uint8_t next_ext_hdr = opt_hdr->next_ext_hdr; + hdr_offset += sizeof(struct gtp_opt); // skip gre opt + + while (next_ext_hdr) + { + if (hdr_offset + 1 > len) + { + return 0; + } + uint8_t length = *((char *)data + hdr_offset) * 4 - 2; + hdr_offset += 1; // skip length field + + if (hdr_offset + length + 1 > len) + { + return 0; + } + hdr_offset += length; // skip data field + next_ext_hdr = *((char *)data + hdr_offset); + hdr_offset += 1; // skip next ext hdr field + } + } + + return hdr_offset; +} + +static inline uint16_t get_gre_hdr_len(const char *data, uint16_t len) +{ + /* + * GRE Header Format (Version 0) + * + * 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 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * |C|R|K|S|s|Recur| Flags | Ver | Protocol Type | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Checksum (optional) | Offset (optional) | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Key (optional) | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Sequence Number (optional) | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Routing (optional) + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * + * 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 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Address Family | SRE Offset | SRE Length | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Routing Information ... + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * + * https://datatracker.ietf.org/doc/html/rfc1701 + * https://datatracker.ietf.org/doc/html/rfc2890 + */ + + /* + * Enhanced GRE header (Version 1) + * + * 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 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * |C|R|K|S|s|Recur|A| Flags | Ver | Protocol Type | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Key (HW) Payload Length | Key (LW) Call ID | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Sequence Number (Optional) | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Acknowledgment Number (Optional) | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * + * https://datatracker.ietf.org/doc/html/rfc2637 + */ + +/* bit positions for flags in header */ +#define GRE_CHECKSUM 0x8000 +#define GRE_ROUTING 0x4000 +#define GRE_KEY 0x2000 +#define GRE_SEQUENCE 0x1000 +#define GRE_STRICTSOURCE 0x0800 +#define GRE_RECURSION 0x0700 +#define GRE_ACK 0x0080 /* only in special PPTPized GRE header */ +#define GRE_RESERVED_PPP 0x0078 /* only in special PPTPized GRE header */ +#define GRE_RESERVED 0x00F8 +#define GRE_VERSION 0x0007 + + if (len < 4) + { + return 0; + } + + struct SRE + { + uint16_t address_family; + uint8_t sre_offset; + uint8_t sre_length; + } __attribute__((__packed__)); + + uint16_t sre_size = sizeof(struct SRE); + const uint16_t *gre = (const uint16_t *)data; + uint16_t flags = ntohs(gre[0]); + uint16_t version = flags & GRE_VERSION; + uint16_t hdr_offset = 0; + + if (version == 0) + { + hdr_offset = 4; + + if ((flags & GRE_CHECKSUM) || (flags & GRE_ROUTING)) + { + hdr_offset += 4; + } + if (flags & GRE_KEY) + { + hdr_offset += 4; + } + if (flags & GRE_SEQUENCE) + { + hdr_offset += 4; + } + if (flags & GRE_ROUTING) + { + while (hdr_offset + sre_size <= len) + { + struct SRE *sre = (struct SRE *)((char *)data + hdr_offset); + if (sre->sre_length == 0) + { + hdr_offset += sre_size; + break; + } + else + { + hdr_offset += sre_size + sre->sre_length; + } + } + } + } + + if (version == 1) + { + hdr_offset = 8; + if (flags & GRE_SEQUENCE) + { + hdr_offset += 4; + } + if (flags & GRE_ACK) + { + hdr_offset += 4; + } + } + + if (hdr_offset > len) + { + return 0; + } + + return hdr_offset; +} + +static inline const char *parse_ether(struct packet *handler, const char *data, uint16_t len) +{ + if (unlikely(len < sizeof(struct ethhdr))) + { + PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_ETHER); + return data; + } + + struct layer_record *layer = get_free_layer(handler); + if (unlikely(layer == NULL)) + { + return data; + } + uint16_t next_proto = ntohs(((struct ethhdr *)data)->h_proto); + SET_LAYER(handler, layer, LAYER_TYPE_ETHER, sizeof(struct ethhdr), data, len); + + // TESTED + return parse_l3(handler, next_proto, layer->pld_ptr, layer->pld_len); +} + +static inline const char *parse_ppp(struct packet *handler, const char *data, uint16_t len) +{ + if (unlikely(len < 4)) + { + PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_PPP); + return data; + } + + struct layer_record *layer = get_free_layer(handler); + if (unlikely(layer == NULL)) + { + return data; + } + uint16_t next_proto = ntohs(*((uint16_t *)data + 1)); + SET_LAYER(handler, layer, LAYER_TYPE_PPP, 4, data, len); + + switch (next_proto) + { + // TESTED + case PPP_IP: + return parse_ipv4(handler, layer->pld_ptr, layer->pld_len); + case PPP_IPV6: + return parse_ipv6(handler, layer->pld_ptr, layer->pld_len); + default: + PACKET_LOG_UNSUPPORT_PROTO("ppp", next_proto); + return layer->pld_ptr; + } +} + +static inline const char *parse_vlan(struct packet *handler, const char *data, uint16_t len) +{ + struct vlan_hdr + { + uint16_t vlan_cfi; + uint16_t protocol; + } __attribute__((__packed__)); + + if (unlikely(len < sizeof(struct vlan_hdr))) + { + PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_VLAN); + return data; + } + + struct layer_record *layer = get_free_layer(handler); + if (unlikely(layer == NULL)) + { + return data; + } + uint16_t next_proto = ntohs(((struct vlan_hdr *)data)->protocol); + SET_LAYER(handler, layer, LAYER_TYPE_VLAN, sizeof(struct vlan_hdr), data, len); + + // TESTED + return parse_l3(handler, next_proto, layer->pld_ptr, layer->pld_len); +} + +static inline const char *parse_pppoe_ses(struct packet *handler, const char *data, uint16_t len) +{ +#define PPPOE_TYPE_IPV4 0x2100 +#define PPPOE_TYPE_IPV6 0x5700 + + if (unlikely(len < 8)) + { + PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_PPPOE); + return data; + } + + struct layer_record *layer = get_free_layer(handler); + if (unlikely(layer == NULL)) + { + return data; + } + uint16_t next_proto = *((uint16_t *)data + 3); + SET_LAYER(handler, layer, LAYER_TYPE_PPPOE, 8, data, len); + + switch (next_proto) + { + // TESTED + case PPPOE_TYPE_IPV4: + return parse_ipv4(handler, layer->pld_ptr, layer->pld_len); + case PPPOE_TYPE_IPV6: + return parse_ipv6(handler, layer->pld_ptr, layer->pld_len); + default: + PACKET_LOG_UNSUPPORT_PROTO("pppoe", next_proto); + return layer->pld_ptr; + } +} + +static inline const char *parse_mpls(struct packet *handler, const char *data, uint16_t len) +{ + /* + * 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; + } + + struct layer_record *layer = get_free_layer(handler); + if (unlikely(layer == NULL)) + { + return data; + } + + uint16_t hdr_len = 4; + 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) + { + case 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 + */ + hdr_len += 4; // skip PW Ethernet Control Word + next_proto = MPLS_NEXT_PROTO_ETHER; + break; + case 4: + next_proto = MPLS_NEXT_PROTO_IPV4; + break; + case 6: + next_proto = MPLS_NEXT_PROTO_IPV6; + break; + default: + next_proto = MPLS_NEXT_PROTO_ETHER; + break; + } + } + else + { + next_proto = MPLS_NEXT_PROTO_MPLS; + } + SET_LAYER(handler, layer, LAYER_TYPE_MPLS, hdr_len, data, len); + + switch (next_proto) + { + // TESTED + case MPLS_NEXT_PROTO_IPV4: + return parse_ipv4(handler, layer->pld_ptr, layer->pld_len); + case MPLS_NEXT_PROTO_IPV6: + return parse_ipv6(handler, layer->pld_ptr, layer->pld_len); + // TESTED + case MPLS_NEXT_PROTO_ETHER: + return parse_ether(handler, layer->pld_ptr, layer->pld_len); + // TESTED + case MPLS_NEXT_PROTO_MPLS: + return parse_mpls(handler, layer->pld_ptr, layer->pld_len); + default: + // unreachable + return layer->pld_ptr; + } +} + +static inline const char *parse_ipv4(struct packet *handler, const char *data, uint16_t len) +{ + if (unlikely(len < sizeof(struct ip))) + { + PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_IPV4); + return data; + } + + struct layer_record *layer = get_free_layer(handler); + if (unlikely(layer == NULL)) + { + return data; + } + struct ip *hdr = (struct ip *)data; + uint8_t next_proto = hdr->ip_p; + uint16_t hdr_len = (hdr->ip_hl & 0xf) * 4u; + SET_LAYER(handler, layer, LAYER_TYPE_IPV4, hdr_len, data, len); + + // ip fragmented + if ((ntohs(hdr->ip_off) & IP_MF) || (ntohs(hdr->ip_off) & IP_OFFMASK)) + { + PACKET_LOG_DEBUG("ip is fragmented"); + return layer->pld_ptr; + } + + // TESTED + return parse_l4(handler, next_proto, layer->pld_ptr, layer->pld_len); +} + +static inline const char *parse_ipv6(struct packet *handler, const char *data, uint16_t len) +{ + if (unlikely(len < sizeof(struct ip6_hdr))) + { + PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_IPV6); + return data; + } + + struct layer_record *layer = get_free_layer(handler); + if (unlikely(layer == NULL)) + { + return data; + } + uint8_t next_proto = ((struct ip6_hdr *)data)->ip6_nxt; + SET_LAYER(handler, layer, LAYER_TYPE_IPV6, sizeof(struct ip6_hdr), data, len); + + // TESTED + return parse_l4(handler, next_proto, layer->pld_ptr, layer->pld_len); +} + +static inline const char *parse_gre(struct packet *handler, const char *data, uint16_t len) +{ +#define GRE_PRO_IPV4 (0x0800) +#define GRE_PRO_IPV6 (0x86DD) +#define GRE_PRO_ARP (0x0806) +#define GRE_PRO_PPP (0x880B) + + uint16_t hdr_len = get_gre_hdr_len(data, len); + if (unlikely(hdr_len == 0)) + { + PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_GRE); + return data; + } + + struct layer_record *layer = get_free_layer(handler); + if (unlikely(layer == NULL)) + { + return data; + } + uint16_t next_proto = ntohs(*((uint16_t *)data + 1)); + SET_LAYER(handler, layer, LAYER_TYPE_GRE, hdr_len, data, len); + + switch (next_proto) + { + case GRE_PRO_IPV4: + return parse_ipv4(handler, layer->pld_ptr, layer->pld_len); + case GRE_PRO_IPV6: + return parse_ipv6(handler, layer->pld_ptr, layer->pld_len); + // TESTED + case GRE_PRO_PPP: + return parse_ppp(handler, layer->pld_ptr, layer->pld_len); + default: + PACKET_LOG_UNSUPPORT_PROTO("gre", next_proto); + return layer->pld_ptr; + } +} + +static inline const char *parse_udp(struct packet *handler, const char *data, uint16_t len) +{ + if (unlikely(len < sizeof(struct udphdr))) + { + PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_UDP); + return data; + } + + struct layer_record *layer = get_free_layer(handler); + if (unlikely(layer == NULL)) + { + return data; + } + struct udphdr *hdr = (struct udphdr *)data; + SET_LAYER(handler, layer, LAYER_TYPE_UDP, sizeof(struct udphdr), data, len); + + switch (ntohs(hdr->uh_dport)) + { + // TESTED + // VXLAN_DPORT + case 4789: + return parse_vxlan(handler, layer->pld_ptr, layer->pld_len); + // TESTED + // GTP1U_PORT + case 2152: + return parse_gtpv1_u(handler, layer->pld_ptr, layer->pld_len); + default: + return layer->pld_ptr; + } +} + +static inline const char *parse_tcp(struct packet *handler, const char *data, uint16_t len) +{ + if (unlikely(len < sizeof(struct tcphdr))) + { + PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_TCP); + return data; + } + + struct layer_record *layer = get_free_layer(handler); + if (unlikely(layer == NULL)) + { + return data; + } + uint16_t hdr_len = ((struct tcphdr *)data)->th_off << 2; + SET_LAYER(handler, layer, LAYER_TYPE_TCP, hdr_len, data, len); + + return layer->pld_ptr; +} + +static inline const char *parse_vxlan(struct packet *handler, const char *data, uint16_t len) +{ + struct vxlan_hdr + { + uint8_t flags[2]; + uint16_t gdp; + uint8_t vni[3]; + uint8_t reserved; + } __attribute__((__packed__)); + + if (unlikely(len < sizeof(struct vxlan_hdr))) + { + PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_VXLAN); + return data; + } + + struct layer_record *layer = get_free_layer(handler); + if (unlikely(layer == NULL)) + { + return data; + } + SET_LAYER(handler, layer, LAYER_TYPE_VXLAN, sizeof(struct vxlan_hdr), data, len); + + // TESTED + return parse_ether(handler, layer->pld_ptr, layer->pld_len); +} + +static inline const char *parse_gtpv1_u(struct packet *handler, const char *data, uint16_t len) +{ + uint16_t hdr_len = get_gtp_hdr_len(data, len); + if (unlikely(hdr_len == 0)) + { + PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_GTPV1_U); + return data; + } + + struct layer_record *layer = get_free_layer(handler); + if (unlikely(layer == NULL)) + { + return data; + } + uint8_t next_proto = (((const uint8_t *)(data + hdr_len))[0]) >> 4; + SET_LAYER(handler, layer, LAYER_TYPE_GTPV1_U, hdr_len, data, len); + + switch (next_proto) + { + // TESTED + case 4: + return parse_ipv4(handler, layer->pld_ptr, layer->pld_len); + // TESTED + case 6: + return parse_ipv6(handler, layer->pld_ptr, layer->pld_len); + default: + PACKET_LOG_UNSUPPORT_PROTO("gtp", next_proto); + return layer->pld_ptr; + } +} + +static inline const char *parse_l3(struct packet *handler, uint16_t next_proto, const char *data, uint16_t len) +{ + switch (next_proto) + { + // TESTED + case ETH_P_8021Q: + case ETH_P_8021AD: + return parse_vlan(handler, data, len); + // TESTED + case ETH_P_IP: + return parse_ipv4(handler, data, len); + // TESTED + case ETH_P_IPV6: + return parse_ipv6(handler, data, len); + // TESTED + case ETH_P_PPP_SES: + return parse_pppoe_ses(handler, data, len); + // TESTED + case ETH_P_MPLS_UC: + return parse_mpls(handler, data, len); + default: + // TESTED ARP + PACKET_LOG_UNSUPPORT_ETHPROTO("l3", next_proto); + return data; + } +} + +static inline const char *parse_l4(struct packet *handler, uint8_t next_proto, const char *data, uint16_t len) +{ + switch (next_proto) + { + // TESTED + case IPPROTO_TCP: + return parse_tcp(handler, data, len); + // TESTED + case IPPROTO_UDP: + return parse_udp(handler, data, len); + // TESTED + case IPPROTO_IPIP: + return parse_ipv4(handler, data, len); + // TESTED + case IPPROTO_IPV6: + return parse_ipv6(handler, data, len); + // TESTED + case IPPROTO_GRE: + return parse_gre(handler, data, len); + default: + PACKET_LOG_UNSUPPORT_IPPROTO("l4", next_proto); + return data; + } +} + +/****************************************************************************** + * Public API + ******************************************************************************/ + +// return innermost payload +const char *packet_parse(struct packet *handler, const char *data, uint16_t len) +{ + handler->layers_used = 0; + handler->layers_size = PACKET_MAX_LAYERS; + handler->data_ptr = data; + handler->data_len = len; + handler->zone_id = 0; + handler->user_data = NULL; + + // TESTED + return parse_ether(handler, data, len); +} + +void packet_print(const struct packet *handler) +{ + if (handler == NULL) + { + return; + } + + printf("packet: %p, data_ptr: %p, data_len: %u, layers_used: %u, layers_size: %u\n", + handler, handler->data_ptr, handler->data_len, + handler->layers_used, handler->layers_size); + for (uint8_t i = 0; i < handler->layers_used; i++) + { + const struct layer_record *layer = &handler->layers[i]; + printf(" layer[%u]: %p, type: %s, hdr_offset: %u, hdr_ptr: %p, hdr_len: %u, pld_ptr: %p, pld_len: %u\n", + i, layer, layer_type_tostring(layer->type), layer->hdr_offset, + layer->hdr_ptr, layer->hdr_len, layer->pld_ptr, layer->pld_len); + } +} + +// return 0 : found +// return -1 : not found +int packet_get_innermost_tuple2(const struct packet *handler, struct tuple2 *tuple) +{ + const struct layer_record *layer = NULL; + + for (int8_t i = handler->layers_used - 1; i >= 0; i--) + { + layer = &handler->layers[i]; + + if (layer->type & LAYER_TYPE_L3) + { + set_tuple2((const char *)handler->data_ptr + layer->hdr_offset, layer->type, tuple); + return 0; + } + } + + return -1; +} + +// return 0 : found +// return -1 : not found +int packet_get_outermost_tuple2(const struct packet *handler, struct tuple2 *tuple) +{ + const struct layer_record *layer = NULL; + + for (int8_t i = 0; i < handler->layers_used; i++) + { + layer = &handler->layers[i]; + + if (layer->type & LAYER_TYPE_L3) + { + set_tuple2((const char *)handler->data_ptr + layer->hdr_offset, layer->type, tuple); + return 0; + } + } + + return -1; +} + +// return 0 : found +// return -1 : not found +int packet_get_innermost_tuple4(const struct packet *handler, struct tuple4 *tuple) +{ + const struct layer_record *layer_l3 = NULL; + const struct layer_record *layer_l4 = NULL; + const struct layer_record *layer = NULL; + + for (int8_t i = handler->layers_used - 1; i >= 0; i--) + { + layer = &handler->layers[i]; + + // first get L4 layer + if (layer->type & LAYER_TYPE_L4) + { + layer_l4 = layer; + continue; + } + + // second get L3 layer + if (layer->type & LAYER_TYPE_L3) + { + layer_l3 = layer; + break; + } + } + + if (layer_l3 && layer_l4) + { + set_tuple4((const char *)handler->data_ptr + layer_l3->hdr_offset, layer_l3->type, tuple); + set_tuple4((const char *)handler->data_ptr + layer_l4->hdr_offset, layer_l4->type, tuple); + return 0; + } + else + { + return -1; + } +} + +// return 0 : found +// return -1 : not found +int packet_get_outermost_tuple4(const struct packet *handler, struct tuple4 *tuple) +{ + const struct layer_record *layer_l3 = NULL; + const struct layer_record *layer_l4 = NULL; + const struct layer_record *layer = NULL; + + for (int8_t i = 0; i < handler->layers_used; i++) + { + layer = &handler->layers[i]; + + // first get L3 layer + if (layer->type & LAYER_TYPE_L3) + { + layer_l3 = layer; + continue; + } + + // second get L4 layer + if (layer->type & LAYER_TYPE_L4) + { + layer_l4 = layer; + break; + } + } + + if (layer_l3 && layer_l4) + { + set_tuple4((const char *)handler->data_ptr + layer_l3->hdr_offset, layer_l3->type, tuple); + set_tuple4((const char *)handler->data_ptr + layer_l4->hdr_offset, layer_l4->type, tuple); + return 0; + } + else + { + return -1; + } +} + +// return 0 : found +// return -1 : not found +int packet_get_innermost_tuple6(const struct packet *handler, struct tuple6 *tuple) +{ + const struct layer_record *layer_l3 = NULL; + const struct layer_record *layer_l4 = NULL; + const struct layer_record *layer = NULL; + + for (int8_t i = handler->layers_used - 1; i >= 0; i--) + { + layer = &handler->layers[i]; + + // first get L4 layer + if (layer->type & LAYER_TYPE_L4) + { + layer_l4 = layer; + continue; + } + + // second get L3 layer + if (layer->type & LAYER_TYPE_L3) + { + layer_l3 = layer; + break; + } + } + + if (layer_l3 && layer_l4) + { + set_tuple6((const char *)handler->data_ptr + layer_l3->hdr_offset, layer_l3->type, tuple, handler->zone_id); + set_tuple6((const char *)handler->data_ptr + layer_l4->hdr_offset, layer_l4->type, tuple, handler->zone_id); + return 0; + } + else + { + return -1; + } +} + +// return 0 : found +// return -1 : not found +int packet_get_outermost_tuple6(const struct packet *handler, struct tuple6 *tuple) +{ + const struct layer_record *layer_l3 = NULL; + const struct layer_record *layer_l4 = NULL; + const struct layer_record *layer = NULL; + + for (int8_t i = 0; i < handler->layers_used; i++) + { + layer = &handler->layers[i]; + + // first get L3 layer + if (layer->type & LAYER_TYPE_L3) + { + layer_l3 = layer; + continue; + } + + // second get L4 layer + if (layer->type & LAYER_TYPE_L4) + { + layer_l4 = layer; + break; + } + } + + if (layer_l3 && layer_l4) + { + set_tuple6((const char *)handler->data_ptr + layer_l3->hdr_offset, layer_l3->type, tuple, handler->zone_id); + set_tuple6((const char *)handler->data_ptr + layer_l4->hdr_offset, layer_l4->type, tuple, handler->zone_id); + return 0; + } + else + { + return -1; + } +} + +const struct layer_record *packet_get_innermost_layer(const struct packet *handler, enum layer_type type) +{ + const struct layer_record *layer = NULL; + + for (int8_t i = handler->layers_used - 1; i >= 0; i--) + { + layer = &handler->layers[i]; + if (layer->type & type) + { + return layer; + } + } + + return NULL; +} + +const struct layer_record *packet_get_outermost_layer(const struct packet *handler, enum layer_type type) +{ + const struct layer_record *layer = NULL; + + for (int8_t i = 0; i < handler->layers_used; i++) + { + layer = &handler->layers[i]; + if (layer->type & type) + { + return layer; + } + } + + return NULL; +} + +// direction 1: E2I +// direction 0: I2E +uint64_t packet_get_hash(const struct packet *handler, enum ldbc_method method, int direction) +{ + uint64_t temp = 0; + uint64_t hash_value = 1; + + int inner_addr_len = 0; + int outer_addr_len = 0; + const char *inner_src_addr = NULL; + const char *inner_dst_addr = NULL; + const char *outer_src_addr = NULL; + const char *outer_dst_addr = NULL; + + struct tuple2 inner_addr; + struct tuple2 outer_addr; + + if (handler == NULL) + { + return hash_value; + } + + if (packet_get_innermost_tuple2(handler, &inner_addr) == -1) + { + return hash_value; + } + + if (packet_get_outermost_tuple2(handler, &outer_addr) == -1) + { + return hash_value; + } + + if (inner_addr.ip_type == IP_TYPE_V4) + { + inner_src_addr = (const char *)&inner_addr.src_addr.v4; + inner_dst_addr = (const char *)&inner_addr.dst_addr.v4; + inner_addr_len = sizeof(struct in_addr); + } + else + { + inner_src_addr = (const char *)&inner_addr.src_addr.v6; + inner_dst_addr = (const char *)&inner_addr.dst_addr.v6; + inner_addr_len = sizeof(struct in6_addr); + } + + if (outer_addr.ip_type == IP_TYPE_V4) + { + outer_src_addr = (const char *)&outer_addr.src_addr.v4; + outer_dst_addr = (const char *)&outer_addr.dst_addr.v4; + outer_addr_len = sizeof(struct in_addr); + } + else + { + outer_src_addr = (const char *)&outer_addr.src_addr.v6; + outer_dst_addr = (const char *)&outer_addr.dst_addr.v6; + outer_addr_len = sizeof(struct in6_addr); + } + + switch (method) + { + case LDBC_METHOD_HASH_INT_IP: + if (direction) + { + // direction 1: E2I + HASH_VALUE(outer_dst_addr, outer_addr_len, hash_value); + } + else + { + // direction 0: I2E + HASH_VALUE(outer_src_addr, outer_addr_len, hash_value); + } + break; + case LDBC_METHOD_HASH_EXT_IP: + if (direction) + { + // direction 1: E2I + HASH_VALUE(outer_src_addr, outer_addr_len, hash_value); + } + else + { + // direction 0: I2E + HASH_VALUE(outer_dst_addr, outer_addr_len, hash_value); + } + break; + case LDBC_METHOD_HASH_INT_IP_AND_EXT_IP: + HASH_VALUE(outer_src_addr, outer_addr_len, hash_value); + HASH_VALUE(outer_dst_addr, outer_addr_len, temp); + hash_value = hash_value ^ temp; + break; + case LDBC_METHOD_HASH_INNERMOST_INT_IP: + if (direction) + { + // direction 1: E2I + HASH_VALUE(inner_dst_addr, inner_addr_len, hash_value); + } + else + { + // direction 0: I2E + HASH_VALUE(inner_src_addr, inner_addr_len, hash_value); + } + break; + case LDBC_METHOD_HASH_INNERMOST_EXT_IP: + if (direction) + { + // direction 1: E2I + HASH_VALUE(inner_src_addr, inner_addr_len, hash_value); + } + else + { + // direction 0: I2E + HASH_VALUE(inner_dst_addr, inner_addr_len, hash_value); + } + break; + default: + return hash_value; + } + + return hash_value; +} |