From c298fdf289f4a98b2f58014180553034c95bf494 Mon Sep 17 00:00:00 2001 From: luwenpeng Date: Fri, 16 Aug 2024 16:12:12 +0800 Subject: [PATCH] refactor: mv eth/gre0/gre1/gtp1/gtp2/ip4/ip6/l2tp/mpls/tcp/udp/vlan/vxlan utils.h to packet_helper.h --- src/ip_reassembly/ip_reassembly.cpp | 3 +- src/ip_reassembly/test/gtest_utils.h | 5 +- src/packet/eth_utils.h | 293 -- src/packet/gre0_utils.h | 425 --- src/packet/gre1_utils.h | 226 -- src/packet/gre_utils.h | 69 - src/packet/gtp1_utils.h | 348 -- src/packet/gtp2_utils.h | 270 -- src/packet/gtp_utils.h | 51 - src/packet/ip4_utils.h | 393 --- src/packet/ip6_utils.h | 221 -- src/packet/l2tp_utils.h | 261 -- src/packet/mpls_utils.h | 84 - src/packet/packet_builder.cpp | 7 +- src/packet/packet_dump.cpp | 12 +- src/packet/packet_helper.h | 2870 +++++++++++++++++ src/packet/packet_parser.cpp | 12 +- src/packet/packet_utils.cpp | 5 +- src/packet/tcp_utils.h | 287 -- src/packet/test/gtest_eth_utils.cpp | 2 +- src/packet/test/gtest_gre0_utils.cpp | 2 +- src/packet/test/gtest_gre1_utils.cpp | 2 +- src/packet/test/gtest_gtp1_utils.cpp | 2 +- src/packet/test/gtest_gtp2_utils.cpp | 2 +- src/packet/test/gtest_ip4_utils.cpp | 2 +- src/packet/test/gtest_ip6_utils.cpp | 2 +- src/packet/test/gtest_l2tp_utils.cpp | 2 +- src/packet/test/gtest_mpls_utils.cpp | 2 +- src/packet/test/gtest_packet_builder.cpp | 7 +- src/packet/test/gtest_tcp_utils.cpp | 2 +- src/packet/test/gtest_tunnel.cpp | 6 +- src/packet/test/gtest_udp_utils.cpp | 2 +- src/packet/test/gtest_vlan_utils.cpp | 2 +- src/packet/test/gtest_vxlan_utils.cpp | 2 +- src/packet/udp_utils.h | 90 - src/packet/vlan_utils.h | 92 - src/packet/vxlan_utils.h | 71 - src/session/session_manager.cpp | 3 +- src/session/test/gtest_filter_tcp_dupkt.cpp | 2 +- .../test/gtest_overload_evict_tcp_sess.cpp | 2 +- .../test/gtest_overload_evict_udp_sess.cpp | 2 +- .../test/gtest_sess_mgr_tcp_reassembly.cpp | 2 +- .../gtest_state_tcp_active_to_closing.cpp | 2 +- .../test/gtest_state_tcp_init_to_opening.cpp | 2 +- .../gtest_state_tcp_opening_to_closing.cpp | 2 +- test/packet_tool/packet_tool.cpp | 7 +- 46 files changed, 2900 insertions(+), 3258 deletions(-) delete mode 100644 src/packet/eth_utils.h delete mode 100644 src/packet/gre0_utils.h delete mode 100644 src/packet/gre1_utils.h delete mode 100644 src/packet/gre_utils.h delete mode 100644 src/packet/gtp1_utils.h delete mode 100644 src/packet/gtp2_utils.h delete mode 100644 src/packet/gtp_utils.h delete mode 100644 src/packet/ip4_utils.h delete mode 100644 src/packet/ip6_utils.h delete mode 100644 src/packet/l2tp_utils.h delete mode 100644 src/packet/mpls_utils.h create mode 100644 src/packet/packet_helper.h delete mode 100644 src/packet/tcp_utils.h delete mode 100644 src/packet/udp_utils.h delete mode 100644 src/packet/vlan_utils.h delete mode 100644 src/packet/vxlan_utils.h diff --git a/src/ip_reassembly/ip_reassembly.cpp b/src/ip_reassembly/ip_reassembly.cpp index ee9aa9e..d9275d9 100644 --- a/src/ip_reassembly/ip_reassembly.cpp +++ b/src/ip_reassembly/ip_reassembly.cpp @@ -5,12 +5,11 @@ #include "log.h" #include "checksum.h" -#include "ip4_utils.h" -#include "ip6_utils.h" #include "crc32_hash.h" #include "packet_private.h" #include "packet_utils.h" #include "packet_parser.h" +#include "packet_helper.h" #include "ip_reassembly.h" #define IP_REASSEMBLE_DEBUG(format, ...) LOG_DEBUG("ip_reassembly", format, ##__VA_ARGS__) diff --git a/src/ip_reassembly/test/gtest_utils.h b/src/ip_reassembly/test/gtest_utils.h index 0026415..c426168 100644 --- a/src/ip_reassembly/test/gtest_utils.h +++ b/src/ip_reassembly/test/gtest_utils.h @@ -7,10 +7,7 @@ extern "C" { #endif -#include "udp_utils.h" -#include "tcp_utils.h" -#include "ip4_utils.h" -#include "ip6_utils.h" +#include "packet_helper.h" #include "ip_reassembly.h" #include "packet_private.h" #include "packet_dump.h" diff --git a/src/packet/eth_utils.h b/src/packet/eth_utils.h deleted file mode 100644 index 0b14bac..0000000 --- a/src/packet/eth_utils.h +++ /dev/null @@ -1,293 +0,0 @@ -#pragma once - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include -#include -#include -#include - -/****************************************************************************** - * 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 -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_8021AD: - 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: - case 0x880B: // PPP：Point-to-Point Protoco - return 1; - default: - return 0; - } -} - -/****************************************************************************** - * print - ******************************************************************************/ - -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_8021AD: - return "ETH_P_8021AD"; - 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"; - case 0x880B: - return "ETH_P_PPP"; - default: - return "ETH_P_UNKNOWN"; - } -} - -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 diff --git a/src/packet/gre0_utils.h b/src/packet/gre0_utils.h deleted file mode 100644 index ea44d48..0000000 --- a/src/packet/gre0_utils.h +++ /dev/null @@ -1,425 +0,0 @@ -#pragma once - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include -#include -#include -#include "eth_utils.h" - -/* - * 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 - */ - -struct gre0_hdr -{ - uint16_t flags; - uint16_t protocol; -}; - -struct sre -{ - uint16_t family; - uint8_t offset; - uint8_t length; -}; - -#define GRE0_FLAG_CHECKSUM 0x8000 -#define GRE0_FLAG_ROUTING 0x4000 -#define GRE0_FLAG_KEY 0x2000 -#define GRE0_FLAG_SEQUENCE 0x1000 -#define GRE0_FLAG_STRICT 0x0800 -#define GRE0_FLAG_RECURSION 0x0700 -#define GRE0_FLAG_VERSION 0x0007 - -/****************************************************************************** - * get - ******************************************************************************/ - -static inline uint16_t gre0_hdr_get_flags(const struct gre0_hdr *hdr) -{ - return ntohs(hdr->flags); -} - -static inline uint8_t gre0_hdr_get_checksum_flag(const struct gre0_hdr *hdr) -{ - return (ntohs(hdr->flags) & GRE0_FLAG_CHECKSUM) >> 15; -} - -static inline uint8_t gre0_hdr_get_routing_flag(const struct gre0_hdr *hdr) -{ - return (ntohs(hdr->flags) & GRE0_FLAG_ROUTING) >> 14; -} - -static inline uint8_t gre0_hdr_get_key_flag(const struct gre0_hdr *hdr) -{ - return (ntohs(hdr->flags) & GRE0_FLAG_KEY) >> 13; -} - -static inline uint8_t gre0_hdr_get_seq_flag(const struct gre0_hdr *hdr) -{ - return (ntohs(hdr->flags) & GRE0_FLAG_SEQUENCE) >> 12; -} - -static inline uint8_t gre0_hdr_get_strict_flag(const struct gre0_hdr *hdr) -{ - return (ntohs(hdr->flags) & GRE0_FLAG_STRICT) >> 11; -} - -static inline uint8_t gre0_hdr_get_version(const struct gre0_hdr *hdr) -{ - return ntohs(hdr->flags) & GRE0_FLAG_VERSION; -} - -// ethernet protocol -static inline uint16_t gre0_hdr_get_proto(const struct gre0_hdr *hdr) -{ - return ntohs(hdr->protocol); -} - -static inline uint16_t gre0_hdr_get_checksum(const struct gre0_hdr *hdr) -{ - if (gre0_hdr_get_checksum_flag(hdr)) - { - const char *ptr = ((const char *)hdr) + 4; - return ntohs(*(uint16_t *)ptr); - } - else - { - return 0; - } -} - -static inline uint16_t gre0_hdr_get_offset(const struct gre0_hdr *hdr) -{ - if (gre0_hdr_get_checksum_flag(hdr)) - { - const char *ptr = ((const char *)hdr) + 6; - return ntohs(*(uint16_t *)ptr); - } - else - { - return 0; - } -} - -static inline uint32_t gre0_hdr_get_key(const struct gre0_hdr *hdr) -{ - if (gre0_hdr_get_key_flag(hdr)) - { - const char *ptr = ((const char *)hdr) + 4; - if (gre0_hdr_get_checksum_flag(hdr)) - { - ptr += 4; - } - - return ntohl(*(uint32_t *)ptr); - } - else - { - return 0; - } -} - -static inline uint32_t gre0_hdr_get_seq(const struct gre0_hdr *hdr) -{ - if (gre0_hdr_get_seq_flag(hdr)) - { - const char *ptr = ((const char *)hdr) + 4; - if (gre0_hdr_get_checksum_flag(hdr)) - { - ptr += 4; - } - - if (gre0_hdr_get_key_flag(hdr)) - { - ptr += 4; - } - - return ntohl(*(uint32_t *)ptr); - } - else - { - return 0; - } -} - -static inline const char *gre0_hdr_get_routing_data(const struct gre0_hdr *hdr) -{ - if (gre0_hdr_get_routing_flag(hdr)) - { - const char *ptr = ((const char *)hdr) + 4; - if (gre0_hdr_get_checksum_flag(hdr)) - { - ptr += 4; - } - - if (gre0_hdr_get_key_flag(hdr)) - { - ptr += 4; - } - - if (gre0_hdr_get_seq_flag(hdr)) - { - ptr += 4; - } - - return ptr; - } - else - { - return NULL; - } -} - -static inline uint16_t gre0_hdr_get_routing_len(const struct gre0_hdr *hdr) -{ - if (gre0_hdr_get_routing_flag(hdr)) - { - const char *ptr = gre0_hdr_get_routing_data(hdr); - - uint16_t hdr_len = 0; - while (1) - { - struct sre *sre = (struct sre *)(ptr + hdr_len); - if (sre->length == 0) - { - hdr_len += sizeof(struct sre); - break; - } - else - { - hdr_len += sizeof(struct sre) + sre->length; - } - } - return hdr_len; - } - else - { - return 0; - } -} - -static inline uint16_t calc_gre0_hdr_len(const char *data, uint32_t len) -{ - if (data == NULL || len < sizeof(struct gre0_hdr)) - { - return 0; - } - const struct gre0_hdr *hdr = (const struct gre0_hdr *)data; - uint16_t hdr_len = 4; - uint16_t flags = ntohs(hdr->flags); - if ((flags & GRE0_FLAG_CHECKSUM) || (flags & GRE0_FLAG_ROUTING)) - { - hdr_len += 4; // skip checksum and offset fields - } - if (flags & GRE0_FLAG_KEY) - { - hdr_len += 4; // skip key field - } - if (flags & GRE0_FLAG_SEQUENCE) - { - hdr_len += 4; // skip sequence number field - } - if (flags & GRE0_FLAG_ROUTING) - { - while (hdr_len + sizeof(struct sre) <= len) - { - struct sre *sre = (struct sre *)((char *)data + hdr_len); - if (sre->length == 0) - { - hdr_len += sizeof(struct sre); - break; - } - else - { - hdr_len += sizeof(struct sre) + sre->length; - } - } - } - return hdr_len; -} - -/****************************************************************************** - * set - ******************************************************************************/ - -static inline void gre0_hdr_set_flags(struct gre0_hdr *hdr, uint16_t flags) -{ - hdr->flags = htons(flags); -} - -static inline void gre0_hdr_set_checksum_flag(struct gre0_hdr *hdr, uint8_t flag) -{ - hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_CHECKSUM) | flag << 15); -} - -static inline void gre0_hdr_set_routing_flag(struct gre0_hdr *hdr, uint8_t flag) -{ - hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_ROUTING) | flag << 14); -} - -static inline void gre0_hdr_set_key_flag(struct gre0_hdr *hdr, uint8_t flag) -{ - hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_KEY) | flag << 13); -} - -static inline void gre0_hdr_set_seq_flag(struct gre0_hdr *hdr, uint8_t flag) -{ - hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_SEQUENCE) | flag << 12); -} - -static inline void gre0_hdr_set_strict_flag(struct gre0_hdr *hdr, uint8_t flag) -{ - hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_STRICT) | flag << 11); -} - -static inline void gre0_hdr_set_version(struct gre0_hdr *hdr, uint8_t version) -{ - hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_VERSION) | version); -} - -static inline void gre0_hdr_set_proto(struct gre0_hdr *hdr, uint16_t proto) -{ - hdr->protocol = htons(proto); -} - -static inline void gre0_hdr_set_checksum(struct gre0_hdr *hdr, uint16_t checksum) -{ - if (gre0_hdr_get_checksum_flag(hdr)) - { - char *ptr = ((char *)hdr) + 4; - *(uint16_t *)ptr = htons(checksum); - } -} - -static inline void gre0_hdr_set_offset(struct gre0_hdr *hdr, uint16_t offset) -{ - if (gre0_hdr_get_checksum_flag(hdr)) - { - char *ptr = ((char *)hdr) + 6; - *(uint16_t *)ptr = htons(offset); - } -} - -static inline void gre0_hdr_set_key(struct gre0_hdr *hdr, uint32_t key) -{ - if (gre0_hdr_get_key_flag(hdr)) - { - char *ptr = ((char *)hdr) + 4; - if (gre0_hdr_get_checksum_flag(hdr)) - { - ptr += 4; - } - - *(uint32_t *)ptr = htonl(key); - } -} - -static inline void gre0_hdr_set_seq(struct gre0_hdr *hdr, uint32_t sequence) -{ - if (gre0_hdr_get_seq_flag(hdr)) - { - char *ptr = ((char *)hdr) + 4; - if (gre0_hdr_get_checksum_flag(hdr)) - { - ptr += 4; - } - - if (gre0_hdr_get_key_flag(hdr)) - { - ptr += 4; - } - - *(uint32_t *)ptr = htonl(sequence); - } -} - -static inline void gre0_hdr_set_routing_data(struct gre0_hdr *hdr, const char *data, uint16_t len) -{ - if (gre0_hdr_get_routing_flag(hdr)) - { - char *ptr = ((char *)hdr) + 4; - if (gre0_hdr_get_checksum_flag(hdr)) - { - ptr += 4; - } - - if (gre0_hdr_get_key_flag(hdr)) - { - ptr += 4; - } - - if (gre0_hdr_get_seq_flag(hdr)) - { - ptr += 4; - } - - memcpy(ptr, data, len); - } -} - -/****************************************************************************** - * print - ******************************************************************************/ - -static inline int gre0_hdr_to_str(const struct gre0_hdr *hdr, char *buf, size_t size) -{ - memset(buf, 0, size); - - int used = 0; - uint16_t proto = gre0_hdr_get_proto(hdr); - used += snprintf(buf + used, size - used, "GRE: flags=0x%04x (checksum_flag=%u, routing_flag=%u, key_flag=%u, seq_flag=%u, strict_flag=%u, version=%u), proto=%s", - gre0_hdr_get_flags(hdr), gre0_hdr_get_checksum_flag(hdr), gre0_hdr_get_routing_flag(hdr), gre0_hdr_get_key_flag(hdr), - gre0_hdr_get_seq_flag(hdr), gre0_hdr_get_strict_flag(hdr), gre0_hdr_get_version(hdr), eth_proto_to_str(proto)); - if (gre0_hdr_get_checksum_flag(hdr)) - { - used += snprintf(buf + used, size - used, ", checksum=0x%x, offset=%u", gre0_hdr_get_checksum(hdr), gre0_hdr_get_offset(hdr)); - } - if (gre0_hdr_get_key_flag(hdr)) - { - used += snprintf(buf + used, size - used, ", key=%u", gre0_hdr_get_key(hdr)); - } - if (gre0_hdr_get_seq_flag(hdr)) - { - used += snprintf(buf + used, size - used, ", seq=%u", gre0_hdr_get_seq(hdr)); - } - if (gre0_hdr_get_routing_flag(hdr)) - { - used += snprintf(buf + used, size - used, ", routing_len=%u", gre0_hdr_get_routing_len(hdr)); - } - - return used; -} - -#ifdef __cplusplus -} -#endif diff --git a/src/packet/gre1_utils.h b/src/packet/gre1_utils.h deleted file mode 100644 index 2dca44b..0000000 --- a/src/packet/gre1_utils.h +++ /dev/null @@ -1,226 +0,0 @@ -#pragma once - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include -#include -#include -#include "eth_utils.h" - -/* - * 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 - */ - -struct gre1_hdr -{ - uint16_t flags; - uint16_t protocol; - uint16_t payload_length; - uint16_t call_id; -}; - -#define GRE1_FLAG_CHECKSUM 0x8000 -#define GRE1_FLAG_ROUTING 0x4000 -#define GRE1_FLAG_KEY 0x2000 -#define GRE1_FLAG_SEQUENCE 0x1000 -#define GRE1_FLAG_STRICT 0x0800 -#define GRE1_FLAG_RECURSION 0x0700 -#define GRE1_FLAG_ACK 0x0080 /* only in special PPTPized GRE header */ -#define GRE1_FLAG_VERSION 0x0007 - -/****************************************************************************** - * get - ******************************************************************************/ - -static inline uint16_t gre1_hdr_get_flags(const struct gre1_hdr *hdr) -{ - return ntohs(hdr->flags); -} - -static inline uint8_t gre1_hdr_get_seq_flag(const struct gre1_hdr *hdr) -{ - return (ntohs(hdr->flags) & GRE1_FLAG_SEQUENCE) >> 12; -} - -static inline uint8_t gre1_hdr_get_ack_flag(const struct gre1_hdr *hdr) -{ - return (ntohs(hdr->flags) & GRE1_FLAG_ACK) >> 7; -} - -static inline uint8_t gre1_hdr_get_version(const struct gre1_hdr *hdr) -{ - return ntohs(hdr->flags) & GRE1_FLAG_VERSION; -} - -// ethernet protocol type -static inline uint16_t gre1_hdr_get_proto(const struct gre1_hdr *hdr) -{ - return ntohs(hdr->protocol); -} - -static inline uint16_t gre1_hdr_get_payload_length(const struct gre1_hdr *hdr) -{ - return ntohs(hdr->payload_length); -} - -static inline uint16_t gre1_hdr_get_call_id(const struct gre1_hdr *hdr) -{ - return ntohs(hdr->call_id); -} - -static inline uint32_t gre1_hdr_get_seq(const struct gre1_hdr *hdr) -{ - if (gre1_hdr_get_seq_flag(hdr)) - { - const char *ptr = ((const char *)hdr) + 8; - return ntohl(*((uint32_t *)ptr)); - } - else - { - return 0; - } -} - -static inline uint32_t gre1_hdr_get_ack(const struct gre1_hdr *hdr) -{ - if (gre1_hdr_get_ack_flag(hdr)) - { - const char *ptr = ((const char *)hdr) + 8; - if (gre1_hdr_get_seq_flag(hdr)) - { - ptr += 4; - } - return ntohl(*((uint32_t *)ptr)); - } - else - { - return 0; - } -} - -static inline uint16_t calc_gre1_hdr_len(const char *data, uint32_t len) -{ - if (data == NULL || len < sizeof(struct gre1_hdr)) - { - return 0; - } - const struct gre1_hdr *hdr = (const struct gre1_hdr *)data; - uint16_t hdr_len = 8; - uint16_t flags = gre1_hdr_get_flags(hdr); - if (flags & GRE1_FLAG_SEQUENCE) - { - hdr_len += 4; - } - if (flags & GRE1_FLAG_ACK) - { - hdr_len += 4; - } - return hdr_len; -} - -/****************************************************************************** - * set - ******************************************************************************/ - -static inline void gre1_hdr_set_flags(struct gre1_hdr *hdr, uint16_t flags) -{ - hdr->flags = htons(flags); -} - -static inline void gre1_hdr_set_seq_flag(struct gre1_hdr *hdr, uint8_t seq_flag) -{ - hdr->flags = htons((ntohs(hdr->flags) & ~GRE1_FLAG_SEQUENCE) | seq_flag << 12); -} - -static inline void gre1_hdr_set_ack_flag(struct gre1_hdr *hdr, uint8_t ack_flag) -{ - hdr->flags = htons((ntohs(hdr->flags) & ~GRE1_FLAG_ACK) | ack_flag << 7); -} - -static inline void gre1_hdr_set_version(struct gre1_hdr *hdr, uint8_t version) -{ - hdr->flags = htons((ntohs(hdr->flags) & ~GRE1_FLAG_VERSION) | version); -} - -static inline void gre1_hdr_set_proto(struct gre1_hdr *hdr, uint16_t proto) -{ - hdr->protocol = htons(proto); -} - -static inline void gre1_hdr_set_payload_length(struct gre1_hdr *hdr, uint16_t payload_length) -{ - hdr->payload_length = htons(payload_length); -} - -static inline void gre1_hdr_set_call_id(struct gre1_hdr *hdr, uint16_t call_id) -{ - hdr->call_id = htons(call_id); -} - -static inline void gre1_hdr_set_seq(struct gre1_hdr *hdr, uint32_t seq) -{ - if (gre1_hdr_get_seq_flag(hdr)) - { - char *ptr = ((char *)hdr) + 8; - *((uint32_t *)ptr) = htonl(seq); - } -} - -static inline void gre1_hdr_set_ack(struct gre1_hdr *hdr, uint32_t ack) -{ - if (gre1_hdr_get_ack_flag(hdr)) - { - char *ptr = ((char *)hdr) + 8; - if (gre1_hdr_get_seq_flag(hdr)) - { - ptr += 4; - } - *((uint32_t *)ptr) = htonl(ack); - } -} - -/****************************************************************************** - * print - ******************************************************************************/ - -static inline int gre1_hdr_to_str(const struct gre1_hdr *hdr, char *buf, size_t size) -{ - memset(buf, 0, size); - - int used = 0; - uint16_t proto = gre1_hdr_get_proto(hdr); - used += snprintf(buf + used, size - used, "GRE: flags=0x%04x (seq_flag=%u ack_flag=%u version=%u), proto=%s, payload_length=%u, call_id=%u", - gre1_hdr_get_flags(hdr), gre1_hdr_get_seq_flag(hdr), gre1_hdr_get_ack_flag(hdr), gre1_hdr_get_version(hdr), - eth_proto_to_str(proto), gre1_hdr_get_payload_length(hdr), gre1_hdr_get_call_id(hdr)); - if (gre1_hdr_get_seq_flag(hdr)) - { - used += snprintf(buf + used, size - used, ", seq=%u", gre1_hdr_get_seq(hdr)); - } - if (gre1_hdr_get_ack_flag(hdr)) - { - used += snprintf(buf + used, size - used, ", ack=%u", gre1_hdr_get_ack(hdr)); - } - - return used; -} - -#ifdef __cplusplus -} -#endif diff --git a/src/packet/gre_utils.h b/src/packet/gre_utils.h deleted file mode 100644 index 640fcec..0000000 --- a/src/packet/gre_utils.h +++ /dev/null @@ -1,69 +0,0 @@ -#pragma once - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include "gre0_utils.h" -#include "gre1_utils.h" - -#ifndef MIN -#define MIN(x, y) ((x) < (y) ? (x) : (y)) -#endif - -// return GRE version 0 or 1 -// return 255 if not GRE -static inline uint8_t peek_gre_version(const char *data, uint32_t len) -{ - if (len < MIN(sizeof(struct gre0_hdr), sizeof(struct gre1_hdr))) - { - return 255; - } - - uint16_t flag = *(uint16_t *)data; - return ntohs(flag) & 0x0007; -} - -static inline uint16_t peek_gre_proto(const char *data, uint32_t len) -{ - switch (peek_gre_version(data, len)) - { - case 0: - return gre0_hdr_get_proto((const struct gre0_hdr *)data); - case 1: - return gre1_hdr_get_proto((const struct gre1_hdr *)data); - default: - return 0; - } -} - -static inline uint16_t calc_gre_hdr_len(const char *data, uint32_t len) -{ - switch (peek_gre_version(data, len)) - { - case 0: - return calc_gre0_hdr_len(data, len); - case 1: - return calc_gre1_hdr_len(data, len); - default: - return 0; - } -} - -static inline int gre_hdr_to_str(const char *hdr, uint16_t len, char *buf, size_t size) -{ - switch (peek_gre_version(hdr, len)) - { - case 0: - return gre0_hdr_to_str((const struct gre0_hdr *)hdr, buf, size); - case 1: - return gre1_hdr_to_str((const struct gre1_hdr *)hdr, buf, size); - default: - return 0; - } -} - -#ifdef __cplusplus -} -#endif diff --git a/src/packet/gtp1_utils.h b/src/packet/gtp1_utils.h deleted file mode 100644 index 23be8c2..0000000 --- a/src/packet/gtp1_utils.h +++ /dev/null @@ -1,348 +0,0 @@ -#pragma once - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include -#include -#include - -/* - * https://en.wikipedia.org/wiki/GPRS_Tunnelling_Protocol - * - * GTP-C: Gateway GPRS Support Nodes (GGSN) <----> Serving GPRS support Nodes (SGSN) - * GTP-U: Radio Access Network <----> Core Network - * - * GTPv1: - * -> GTPv1-C - * -> GTPv1-U - */ - -/* - * GTP 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 - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Ver |T|R|E|S|N| Message Type | Message Length | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | TEID | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Sequence Number(O) |N-PDU Number(O)|Next Ext Hdr(O)| (optional headers) - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * - * Message Length: - * a 16-bit field that indicates the length of the payload in bytes - * (rest of the packet following the mandatory 8-byte GTP header). - * Includes the optional fields. - */ - -/* - * Next Extension Headers - * - * 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 - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Length | Contents | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | ... | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Contents | Next Ext Hdr | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * - * Extension length - * an 8-bit field. This field states the length of this extension header, - * including the length, the contents, and the next extension header field, - * in 4-octet units, so the length of the extension must always be a multiple of 4. - */ - -struct gtp1_hdr -{ - uint8_t flags; - uint8_t msg_type; - uint16_t msg_len; - uint32_t teid; -} __attribute__((packed)); - -struct gtp1_hdr_long -{ - uint8_t flags; - uint8_t msg_type; - uint16_t msg_len; - uint32_t teid; - - uint16_t seq; - uint8_t npdu; - uint8_t next_ext_hdr; -} __attribute__((packed)); - -#define GTP1_FLAG_N_PDU (0x01) -#define GTP1_FLAG_SEQ_NUM (0x02) -#define GTP1_FLAG_EXT_HDR (0x04) -#define GTP1_FLAG_RESERVED (0x08) -#define GTP1_FLAG_PROTOCOL (0x10) -#define GTP1_FLAG_VERSION (0xE0) - -/****************************************************************************** - * get - ******************************************************************************/ - -static inline uint8_t gtp1_hdr_get_flags(const struct gtp1_hdr *gtp) -{ - return gtp->flags; -} - -static inline uint8_t gtp1_hdr_get_version(const struct gtp1_hdr *gtp) -{ - return (gtp->flags & GTP1_FLAG_VERSION) >> 5; -} - -static inline uint8_t gtp1_hdr_get_proto(const struct gtp1_hdr *gtp) -{ - return (gtp->flags & GTP1_FLAG_PROTOCOL) >> 4; -} - -static inline uint8_t gtp1_hdr_get_reserved(const struct gtp1_hdr *gtp) -{ - return (gtp->flags & GTP1_FLAG_RESERVED) >> 3; -} - -static inline uint8_t gtp1_hdr_get_ext_flag(const struct gtp1_hdr *gtp) -{ - return (gtp->flags & GTP1_FLAG_EXT_HDR) >> 2; -} - -static inline uint8_t gtp1_hdr_get_seq_flag(const struct gtp1_hdr *gtp) -{ - return (gtp->flags & GTP1_FLAG_SEQ_NUM) >> 1; -} - -static inline uint8_t gtp1_hdr_get_npdu_flag(const struct gtp1_hdr *gtp) -{ - return (gtp->flags & GTP1_FLAG_N_PDU) >> 0; -} - -static inline uint8_t gtp1_hdr_get_msg_type(const struct gtp1_hdr *gtp) -{ - return gtp->msg_type; -} - -static inline uint16_t gtp1_hdr_get_msg_len(const struct gtp1_hdr *gtp) -{ - return ntohs(gtp->msg_len); -} - -static inline uint32_t gtp1_hdr_get_teid(const struct gtp1_hdr *gtp) -{ - return ntohl(gtp->teid); -} - -static inline uint16_t gtp1_hdr_get_seq(const struct gtp1_hdr *gtp) -{ - if (gtp1_hdr_get_seq_flag(gtp)) - { - const struct gtp1_hdr_long *gtp_long = (const struct gtp1_hdr_long *)gtp; - return ntohs(gtp_long->seq); - } - else - { - return 0; - } -} - -static inline uint8_t gtp1_hdr_get_npdu(const struct gtp1_hdr *gtp) -{ - if (gtp1_hdr_get_npdu_flag(gtp)) - { - const struct gtp1_hdr_long *gtp_long = (const struct gtp1_hdr_long *)gtp; - return gtp_long->npdu; - } - else - { - return 0; - } -} - -static inline uint8_t gtp1_hdr_get_next_ext_type(const struct gtp1_hdr *gtp) -{ - if (gtp1_hdr_get_ext_flag(gtp)) - { - const struct gtp1_hdr_long *gtp_long = (const struct gtp1_hdr_long *)gtp; - return gtp_long->next_ext_hdr; - } - else - { - return 0; - } -} - -// include gtp fixed header and optional headers and extension headers -static inline uint16_t calc_gtp1_hdr_len(const char *data, uint16_t len) -{ - if (data == NULL || len < sizeof(struct gtp1_hdr)) - { - return 0; - } - - const struct gtp1_hdr *gtp = (const struct gtp1_hdr *)data; - if (gtp1_hdr_get_version(gtp) != 1) - { - return 0; - } - - if (gtp1_hdr_get_flags(gtp) & (GTP1_FLAG_SEQ_NUM | GTP1_FLAG_N_PDU | GTP1_FLAG_EXT_HDR)) - { - if (sizeof(struct gtp1_hdr_long) > len) - { - return 0; - } - - const struct gtp1_hdr_long *gtp_long = (const struct gtp1_hdr_long *)data; - uint8_t next_ext_hdr = gtp_long->next_ext_hdr; - uint16_t offset = sizeof(struct gtp1_hdr_long); - - while (next_ext_hdr) - { - if (offset + 1 > len) - { - return 0; - } - uint16_t ext_hdr_len = *((char *)data + offset) * 4; - if (ext_hdr_len == 0 || offset + ext_hdr_len > len) - { - return 0; - } - offset += ext_hdr_len; // skip extension header - next_ext_hdr = *((char *)data + offset - 1); - } - - return offset; - } - else - { - return sizeof(struct gtp1_hdr); - } -} - -/****************************************************************************** - * set - ******************************************************************************/ - -static inline void gtp1_hdr_set_flags(struct gtp1_hdr *gtp, uint8_t flags) -{ - gtp->flags = flags; -} - -static inline void gtp1_hdr_set_version(struct gtp1_hdr *gtp, uint8_t version) -{ - gtp->flags = (gtp->flags & ~GTP1_FLAG_VERSION) | (version << 5); -} - -static inline void gtp1_hdr_set_proto(struct gtp1_hdr *gtp, uint8_t protocol_type) -{ - gtp->flags = (gtp->flags & ~GTP1_FLAG_PROTOCOL) | (protocol_type << 4); -} - -static inline void gtp1_hdr_set_reserved(struct gtp1_hdr *gtp, uint8_t reserved) -{ - gtp->flags = (gtp->flags & ~GTP1_FLAG_RESERVED) | (reserved << 3); -} - -static inline void gtp1_hdr_set_ext_flag(struct gtp1_hdr *gtp, uint8_t ext_flag) -{ - gtp->flags = (gtp->flags & ~GTP1_FLAG_EXT_HDR) | (ext_flag << 2); -} - -static inline void gtp1_hdr_set_seq_flag(struct gtp1_hdr *gtp, uint8_t seq_flag) -{ - gtp->flags = (gtp->flags & ~GTP1_FLAG_SEQ_NUM) | (seq_flag << 1); -} - -static inline void gtp1_hdr_set_npdu_flag(struct gtp1_hdr *gtp, uint8_t npdu_flag) -{ - gtp->flags = (gtp->flags & ~GTP1_FLAG_N_PDU) | (npdu_flag << 0); -} - -static inline void gtp1_hdr_set_msg_type(struct gtp1_hdr *gtp, uint8_t msg_type) -{ - gtp->msg_type = msg_type; -} - -static inline void gtp1_hdr_set_msg_len(struct gtp1_hdr *gtp, uint16_t msg_len) -{ - gtp->msg_len = htons(msg_len); -} - -static inline void gtp1_hdr_set_teid(struct gtp1_hdr *gtp, uint32_t teid) -{ - gtp->teid = htonl(teid); -} - -static inline void gtp1_hdr_set_seq(struct gtp1_hdr *gtp, uint16_t seq) -{ - if (gtp1_hdr_get_seq_flag(gtp)) - { - struct gtp1_hdr_long *gtp_long = (struct gtp1_hdr_long *)gtp; - gtp_long->seq = htons(seq); - } -} - -static inline void gtp1_hdr_set_npdu(struct gtp1_hdr *gtp, uint8_t npdu) -{ - if (gtp1_hdr_get_npdu_flag(gtp)) - { - struct gtp1_hdr_long *gtp_long = (struct gtp1_hdr_long *)gtp; - gtp_long->npdu = npdu; - } -} - -static inline void gtp1_hdr_set_next_ext_type(struct gtp1_hdr *gtp, uint8_t next_ext_hdr) -{ - if (gtp1_hdr_get_ext_flag(gtp)) - { - struct gtp1_hdr_long *gtp_long = (struct gtp1_hdr_long *)gtp; - gtp_long->next_ext_hdr = next_ext_hdr; - } -} - -/****************************************************************************** - * print - ******************************************************************************/ - -static inline int gtp1_hdr_to_str(const struct gtp1_hdr *gtp, char *buf, size_t len) -{ - memset(buf, 0, len); - - int used = 0; - used += snprintf(buf + used, len - used, "GTP: flags=0x%02x (version=%u, protocol=%u, reserved=%u, ext_flag=%u, seq_flag=%u, npdu_flag=%u), msg_type=0x%02x, msg_len=%u, teid=%u", - gtp1_hdr_get_flags(gtp), - gtp1_hdr_get_version(gtp), - gtp1_hdr_get_proto(gtp), - gtp1_hdr_get_reserved(gtp), - gtp1_hdr_get_ext_flag(gtp), - gtp1_hdr_get_seq_flag(gtp), - gtp1_hdr_get_npdu_flag(gtp), - gtp1_hdr_get_msg_type(gtp), - gtp1_hdr_get_msg_len(gtp), - gtp1_hdr_get_teid(gtp)); - - if (gtp1_hdr_get_seq_flag(gtp)) - { - used += snprintf(buf + used, len - used, ", seq=%u", gtp1_hdr_get_seq(gtp)); - } - if (gtp1_hdr_get_npdu_flag(gtp)) - { - used += snprintf(buf + used, len - used, ", npdu=%u", gtp1_hdr_get_npdu(gtp)); - } - if (gtp1_hdr_get_ext_flag(gtp)) - { - used += snprintf(buf + used, len - used, ", next_ext_hdr=%u", gtp1_hdr_get_next_ext_type(gtp)); - } - return used; -} - -#ifdef __cplusplus -} -#endif diff --git a/src/packet/gtp2_utils.h b/src/packet/gtp2_utils.h deleted file mode 100644 index c7e6cf5..0000000 --- a/src/packet/gtp2_utils.h +++ /dev/null @@ -1,270 +0,0 @@ -#pragma once - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include -#include -#include - -/* - * https://en.wikipedia.org/wiki/GPRS_Tunnelling_Protocol - * https://www.etsi.org/deliver/etsi_ts/129200_129299/129274/08.04.00_60/ts_129274v080400p.pdf - * - * GTP-C: Gateway GPRS Support Nodes (GGSN) <----> Serving GPRS support Nodes (SGSN) - * GTP-U: Radio Access Network <----> Core Network - * - * GTPv2: - * -> GTPv2-C - * -> (no GTPv2-U) - */ - -/* - * GTP version 2 - * - * 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 - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Ver |P|T|spare| Message Type | Message Length | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | TEID (only if T=1) | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Sequence Number | Spare | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * - * Message length: - * This field shall indicate the length of the message in octets excluding the mandatory of the GTP-C header (the first 4 octets). - * The TEID (if present) and the Sequence Number shall be included in the length count. - */ - -struct gtp2_hdr -{ - uint8_t flags; - uint8_t msg_type; - uint16_t msg_len; - uint32_t seq_and_spare; -} __attribute__((packed)); - -struct gtp2_hdr_long -{ - uint8_t flags; - uint8_t msg_type; - uint16_t msg_len; - uint32_t teid; - uint32_t seq_and_spare; -} __attribute__((packed)); - -#define GPT2_FLAG_SPARE (0x07) -#define GTP2_FLAG_TEID (0x08) -#define GTP2_FLAG_PIGGYBACK (0x10) -#define GTP2_FLAG_VERSION (0xE0) - -/****************************************************************************** - * get - ******************************************************************************/ - -static inline uint8_t gtp2_hdr_get_flags(const struct gtp2_hdr *gtp) -{ - return gtp->flags; -} - -static inline uint8_t gtp2_hdr_get_version(const struct gtp2_hdr *gtp) -{ - return (gtp->flags & GTP2_FLAG_VERSION) >> 5; -} - -static inline uint8_t gtp2_hdr_get_piggyback_flag(const struct gtp2_hdr *gtp) -{ - return (gtp->flags & GTP2_FLAG_PIGGYBACK) >> 4; -} - -static inline uint8_t gtp2_hdr_get_teid_flag(const struct gtp2_hdr *gtp) -{ - return (gtp->flags & GTP2_FLAG_TEID) >> 3; -} - -static inline uint8_t gtp2_hdr_get_spare_flag(const struct gtp2_hdr *gtp) -{ - return (gtp->flags & GPT2_FLAG_SPARE) >> 0; -} - -static inline uint8_t gtp2_hdr_get_msg_type(const struct gtp2_hdr *gtp) -{ - return gtp->msg_type; -} - -static inline uint16_t gtp2_hdr_get_msg_len(const struct gtp2_hdr *gtp) -{ - return ntohs(gtp->msg_len); -} - -static inline uint32_t gtp2_hdr_get_teid(const struct gtp2_hdr *gtp) -{ - if (gtp2_hdr_get_teid_flag(gtp)) - { - const struct gtp2_hdr_long *gtp_long = (const struct gtp2_hdr_long *)gtp; - return ntohl(gtp_long->teid); - } - else - { - return 0; - } -} - -static inline uint32_t gtp2_hdr_get_seq(const struct gtp2_hdr *gtp) -{ - if (gtp2_hdr_get_teid_flag(gtp)) - { - const struct gtp2_hdr_long *gtp_long = (const struct gtp2_hdr_long *)gtp; - return ntohl(gtp_long->seq_and_spare) >> 8; - } - else - { - return ntohl(gtp->seq_and_spare) >> 8; - } -} - -static inline uint8_t gtp2_hdr_get_spare(const struct gtp2_hdr *gtp) -{ - if (gtp2_hdr_get_teid_flag(gtp)) - { - const struct gtp2_hdr_long *gtp_long = (const struct gtp2_hdr_long *)gtp; - return ntohl(gtp_long->seq_and_spare) & 0xFF; - } - else - { - return ntohl(gtp->seq_and_spare) & 0xFF; - } -} - -static inline uint16_t calc_gtp2_hdr_len(const char *data, uint16_t len) -{ - if (len < sizeof(struct gtp2_hdr)) - { - return 0; - } - - const struct gtp2_hdr *gtp = (const struct gtp2_hdr *)data; - if (gtp2_hdr_get_version(gtp) == 2) - { - if (gtp2_hdr_get_teid_flag(gtp)) - { - return sizeof(struct gtp2_hdr_long); - } - else - { - return sizeof(struct gtp2_hdr); - } - } - - return 0; -} - -/****************************************************************************** - * set - ******************************************************************************/ - -static inline void gtp2_hdr_set_flags(struct gtp2_hdr *gtp, uint8_t flags) -{ - gtp->flags = flags; -} - -static inline void gtp2_hdr_set_version(struct gtp2_hdr *gtp, uint8_t version) -{ - gtp->flags = (gtp->flags & ~GTP2_FLAG_VERSION) | (version << 5); -} - -static inline void gtp2_hdr_set_piggyback_flag(struct gtp2_hdr *gtp, uint8_t piggyback) -{ - gtp->flags = (gtp->flags & ~GTP2_FLAG_PIGGYBACK) | (piggyback << 4); -} - -static inline void gtp2_hdr_set_teid_flag(struct gtp2_hdr *gtp, uint8_t teid_flag) -{ - gtp->flags = (gtp->flags & ~GTP2_FLAG_TEID) | (teid_flag << 3); -} - -static inline void gtp2_hdr_set_spare_flag(struct gtp2_hdr *gtp, uint8_t spare_flag) -{ - gtp->flags = (gtp->flags & ~GPT2_FLAG_SPARE) | (spare_flag << 0); -} - -static inline void gtp2_hdr_set_msg_type(struct gtp2_hdr *gtp, uint8_t msg_type) -{ - gtp->msg_type = msg_type; -} - -static inline void gtp2_hdr_set_msg_len(struct gtp2_hdr *gtp, uint16_t msg_len) -{ - gtp->msg_len = htons(msg_len); -} - -static inline void gtp2_hdr_set_teid(struct gtp2_hdr *gtp, uint32_t teid) -{ - if (gtp2_hdr_get_teid_flag(gtp)) - { - struct gtp2_hdr_long *gtp_long = (struct gtp2_hdr_long *)gtp; - gtp_long->teid = htonl(teid); - } -} - -static inline void gtp2_hdr_set_seq(struct gtp2_hdr *gtp, uint32_t seq) -{ - if (gtp2_hdr_get_teid_flag(gtp)) - { - struct gtp2_hdr_long *gtp_long = (struct gtp2_hdr_long *)gtp; - gtp_long->seq_and_spare = htonl((seq << 8) | gtp2_hdr_get_spare(gtp)); - } - else - { - gtp->seq_and_spare = htonl((seq << 8) | gtp2_hdr_get_spare(gtp)); - } -} - -static inline void gtp2_hdr_set_spare(struct gtp2_hdr *gtp, uint8_t spare) -{ - if (gtp2_hdr_get_teid_flag(gtp)) - { - struct gtp2_hdr_long *gtp_long = (struct gtp2_hdr_long *)gtp; - gtp_long->seq_and_spare = htonl((gtp2_hdr_get_seq(gtp) << 8) | spare); - } - else - { - gtp->seq_and_spare = htonl((gtp2_hdr_get_seq(gtp) << 8) | spare); - } -} - -/****************************************************************************** - * print - ******************************************************************************/ - -static inline int gtp2_hdr_to_str(const struct gtp2_hdr *gtp, char *buf, size_t len) -{ - memset(buf, 0, len); - - int used = 0; - used += snprintf(buf + used, len - used, "GTP: flags=0x%02x (version=%u, piggyback=%u, teid_flag=%u, spare_flag=%u), msg_type=0x%02x, msg_len=%u", - gtp2_hdr_get_flags(gtp), - gtp2_hdr_get_version(gtp), - gtp2_hdr_get_piggyback_flag(gtp), - gtp2_hdr_get_teid_flag(gtp), - gtp2_hdr_get_spare_flag(gtp), - gtp2_hdr_get_msg_type(gtp), - gtp2_hdr_get_msg_len(gtp)); - - if (gtp2_hdr_get_teid_flag(gtp)) - { - used += snprintf(buf + used, len - used, ", teid=%u", gtp2_hdr_get_teid(gtp)); - } - else - { - used += snprintf(buf + used, len - used, ", seq=%u, spare=%u", gtp2_hdr_get_seq(gtp), gtp2_hdr_get_spare(gtp)); - } - return used; -} - -#ifdef __cplusplus -} -#endif diff --git a/src/packet/gtp_utils.h b/src/packet/gtp_utils.h deleted file mode 100644 index 8b0fc00..0000000 --- a/src/packet/gtp_utils.h +++ /dev/null @@ -1,51 +0,0 @@ -#pragma once - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include "gtp1_utils.h" -#include "gtp2_utils.h" - -// return GTP version 0 or 1 -// return 255 if not GTP -static inline uint8_t peek_gtp_version(const char *data, uint16_t len) -{ - if (data == NULL || len == 0) - { - return 255; - } - - return ((*(uint8_t *)data) >> 5) & 0x07; -} - -static inline uint16_t calc_gtp_hdr_len(const char *data, uint32_t len) -{ - switch (peek_gtp_version(data, len)) - { - case 1: - return calc_gtp1_hdr_len(data, len); - case 2: - return calc_gtp2_hdr_len(data, len); - default: - return 0; - } -} - -static inline int gtp_hdr_to_str(const char *hdr, uint16_t len, char *buf, size_t size) -{ - switch (peek_gtp_version(hdr, len)) - { - case 1: - return gtp1_hdr_to_str((const struct gtp1_hdr *)hdr, buf, size); - case 2: - return gtp2_hdr_to_str((const struct gtp2_hdr *)hdr, buf, size); - default: - return 0; - } -} - -#ifdef __cplusplus -} -#endif diff --git a/src/packet/ip4_utils.h b/src/packet/ip4_utils.h deleted file mode 100644 index 9ce17a7..0000000 --- a/src/packet/ip4_utils.h +++ /dev/null @@ -1,393 +0,0 @@ -#pragma once - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include -#include -#include -#include - -/* - * Internet Header 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 - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * |Version| IHL |Type of Service| Total Length | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Identification |Flags| Fragment Offset | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Time to Live | Protocol | Header Checksum | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Source Address | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Destination Address | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Options | Padding | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - */ - -/****************************************************************************** - * get - ******************************************************************************/ - -static inline uint8_t ip4_hdr_get_version(const struct ip *hdr) -{ - return hdr->ip_v; -} - -// IP Options are included in the hdr_len field -static inline uint8_t ip4_hdr_get_hdr_len(const struct ip *hdr) -{ - return hdr->ip_hl << 2; -} - -static inline uint8_t ip4_hdr_get_tos(const struct ip *hdr) -{ - return hdr->ip_tos; -} - -static inline uint16_t ip4_hdr_get_total_len(const struct ip *hdr) -{ - return ntohs(hdr->ip_len); -} - -static inline uint16_t ip4_hdr_get_ipid(const struct ip *hdr) -{ - return ntohs(hdr->ip_id); -} - -static inline uint8_t ip4_hdr_get_flags(const struct ip *hdr) -{ - return (ntohs(hdr->ip_off) & (~IP_OFFMASK)) >> 13; -} - -static inline bool ip4_hdr_get_rf_flag(const struct ip *hdr) -{ - return (ntohs(hdr->ip_off) & IP_RF) != 0; -} - -static inline bool ip4_hdr_get_df_flag(const struct ip *hdr) -{ - return (ntohs(hdr->ip_off) & IP_DF) != 0; -} - -static inline bool ip4_hdr_get_mf_flag(const struct ip *hdr) -{ - return (ntohs(hdr->ip_off) & IP_MF) != 0; -} - -static inline uint16_t ip4_hdr_get_frag_offset(const struct ip *hdr) -{ - return (ntohs(hdr->ip_off) & IP_OFFMASK) << 3; -} - -static inline uint8_t ip4_hdr_get_ttl(const struct ip *hdr) -{ - return hdr->ip_ttl; -} - -static inline uint8_t ip4_hdr_get_proto(const struct ip *hdr) -{ - return hdr->ip_p; -} - -static inline uint16_t ip4_hdr_get_checksum(const struct ip *hdr) -{ - return ntohs(hdr->ip_sum); -} - -static inline uint32_t ip4_hdr_get_src_addr(const struct ip *hdr) -{ - return ntohl(hdr->ip_src.s_addr); -} - -static inline uint32_t ip4_hdr_get_dst_addr(const struct ip *hdr) -{ - return ntohl(hdr->ip_dst.s_addr); -} - -static inline struct in_addr ip4_hdr_get_src_in_addr(const struct ip *hdr) -{ - return hdr->ip_src; -} - -static inline struct in_addr ip4_hdr_get_dst_in_addr(const struct ip *hdr) -{ - return hdr->ip_dst; -} - -static inline uint8_t ip4_hdr_get_opt_len(const struct ip *hdr) -{ - return ip4_hdr_get_hdr_len(hdr) - sizeof(struct ip); -} - -static inline const char *ip4_hdr_get_opt_data(const struct ip *hdr) -{ - if (ip4_hdr_get_opt_len(hdr) == 0) - { - return NULL; - } - - return (const char *)hdr + sizeof(struct ip); -} - -/****************************************************************************** - * set - ******************************************************************************/ - -static inline void ip4_hdr_set_version(struct ip *hdr, uint8_t version) -{ - hdr->ip_v = version; -} - -static inline void ip4_hdr_set_hdr_len(struct ip *hdr, uint8_t hdr_len) -{ - hdr->ip_hl = hdr_len >> 2; -} - -static inline void ip4_hdr_set_tos(struct ip *hdr, uint8_t tos) -{ - hdr->ip_tos = tos; -} - -static inline void ip4_hdr_set_total_len(struct ip *hdr, uint16_t total_len) -{ - hdr->ip_len = htons(total_len); -} - -static inline void ip4_hdr_set_ipid(struct ip *hdr, uint16_t ipid) -{ - hdr->ip_id = htons(ipid); -} - -static inline void ip4_hdr_set_flags(struct ip *hdr, uint8_t flags) -{ - hdr->ip_off = htons((flags << 13) | (ntohs(hdr->ip_off) & IP_OFFMASK)); -} - -static inline void ip4_hdr_set_rf_flag(struct ip *hdr, bool flag) -{ - if (flag) - { - hdr->ip_off = htons(ntohs(hdr->ip_off) | IP_RF); - } - else - { - hdr->ip_off = htons(ntohs(hdr->ip_off) & ~IP_RF); - } -} - -static inline void ip4_hdr_set_df_flag(struct ip *hdr, bool flag) -{ - if (flag) - { - hdr->ip_off = htons(ntohs(hdr->ip_off) | IP_DF); - } - else - { - hdr->ip_off = htons(ntohs(hdr->ip_off) & ~IP_DF); - } -} - -static inline void ip4_hdr_set_mf_flag(struct ip *hdr, bool flag) -{ - if (flag) - { - hdr->ip_off = htons(ntohs(hdr->ip_off) | IP_MF); - } - else - { - hdr->ip_off = htons(ntohs(hdr->ip_off) & ~IP_MF); - } -} - -static inline void ip4_hdr_set_frag_offset(struct ip *hdr, uint16_t frag_offset) -{ - hdr->ip_off = htons((frag_offset >> 3) | (ntohs(hdr->ip_off) & ~IP_OFFMASK)); -} - -static inline void ip4_hdr_set_ttl(struct ip *hdr, uint8_t ttl) -{ - hdr->ip_ttl = ttl; -} - -static inline void ip4_hdr_set_protocol(struct ip *hdr, uint8_t protocol) -{ - hdr->ip_p = protocol; -} - -static inline void ip4_hdr_set_checksum(struct ip *hdr, uint16_t checksum) -{ - hdr->ip_sum = htons(checksum); -} - -static inline void ip4_hdr_set_src_addr(struct ip *hdr, uint32_t saddr) -{ - hdr->ip_src.s_addr = htonl(saddr); -} - -static inline void ip4_hdr_set_dst_addr(struct ip *hdr, uint32_t daddr) -{ - hdr->ip_dst.s_addr = htonl(daddr); -} - -static inline void ip4_hdr_set_src_in_addr(struct ip *hdr, struct in_addr saddr) -{ - hdr->ip_src = saddr; -} - -static inline void ip4_hdr_set_dst_in_addr(struct ip *hdr, struct in_addr daddr) -{ - hdr->ip_dst = daddr; -} - -static inline void ip4_hdr_set_opt_len(struct ip *hdr, uint8_t opt_len) -{ - ip4_hdr_set_hdr_len(hdr, opt_len + sizeof(struct ip)); -} - -// must be called after ip4_hdr_set_opt_len -static inline void ip4_hdr_set_opt_data(struct ip *hdr, const char *opt_data) -{ - if (opt_data) - { - memcpy((char *)hdr + sizeof(struct ip), opt_data, ip4_hdr_get_opt_len(hdr)); - } -} - -/****************************************************************************** - * identify - ******************************************************************************/ - -// /usr/include/netinet/in.h -static inline int is_ip_proto(uint16_t proto) -{ - switch (proto) - { - case IPPROTO_IP: - case IPPROTO_ICMP: - case IPPROTO_IGMP: - case IPPROTO_IPIP: - case IPPROTO_TCP: - case IPPROTO_EGP: - case IPPROTO_PUP: - case IPPROTO_UDP: - case IPPROTO_IDP: - case IPPROTO_TP: - case IPPROTO_DCCP: - case IPPROTO_IPV6: - case IPPROTO_RSVP: - case IPPROTO_GRE: - case IPPROTO_ESP: - case IPPROTO_AH: - case IPPROTO_MTP: - case IPPROTO_BEETPH: - case IPPROTO_ENCAP: - case IPPROTO_PIM: - case IPPROTO_COMP: - case IPPROTO_SCTP: - case IPPROTO_UDPLITE: - case IPPROTO_MPLS: - case IPPROTO_ETHERNET: - case IPPROTO_RAW: - case IPPROTO_MPTCP: - return 1; - default: - return 0; - } -} - -/****************************************************************************** - * print - ******************************************************************************/ - -static inline const char *ip_proto_to_str(uint16_t proto) -{ - switch (proto) - { - case IPPROTO_IP: - return "IPPROTO_IP"; - case IPPROTO_ICMP: - return "IPPROTO_ICMP"; - case IPPROTO_IGMP: - return "IPPROTO_IGMP"; - case IPPROTO_IPIP: - return "IPPROTO_IPIP"; - case IPPROTO_TCP: - return "IPPROTO_TCP"; - case IPPROTO_EGP: - return "IPPROTO_EGP"; - case IPPROTO_PUP: - return "IPPROTO_PUP"; - case IPPROTO_UDP: - return "IPPROTO_UDP"; - case IPPROTO_IDP: - return "IPPROTO_IDP"; - case IPPROTO_TP: - return "IPPROTO_TP"; - case IPPROTO_DCCP: - return "IPPROTO_DCCP"; - case IPPROTO_IPV6: - return "IPPROTO_IPV6"; - case IPPROTO_RSVP: - return "IPPROTO_RSVP"; - case IPPROTO_GRE: - return "IPPROTO_GRE"; - case IPPROTO_ESP: - return "IPPROTO_ESP"; - case IPPROTO_AH: - return "IPPROTO_AH"; - case IPPROTO_MTP: - return "IPPROTO_MTP"; - case IPPROTO_BEETPH: - return "IPPROTO_BEETPH"; - case IPPROTO_ENCAP: - return "IPPROTO_ENCAP"; - case IPPROTO_PIM: - return "IPPROTO_PIM"; - case IPPROTO_COMP: - return "IPPROTO_COMP"; - case IPPROTO_SCTP: - return "IPPROTO_SCTP"; - case IPPROTO_UDPLITE: - return "IPPROTO_UDPLITE"; - case IPPROTO_MPLS: - return "IPPROTO_MPLS"; - case IPPROTO_ETHERNET: - return "IPPROTO_ETHERNET"; - case IPPROTO_RAW: - return "IPPROTO_RAW"; - case IPPROTO_MPTCP: - return "IPPROTO_MPTCP"; - default: - return "IPPROTO_UNKNOWN"; - } -} - -static inline int ip4_hdr_to_str(const struct ip *hdr, char *buf, size_t size) -{ - memset(buf, 0, size); - char src_addr_str[INET6_ADDRSTRLEN] = {0}; - char dst_addr_str[INET6_ADDRSTRLEN] = {0}; - - uint16_t proto = ip4_hdr_get_proto(hdr); - struct in_addr src_addr = ip4_hdr_get_src_in_addr(hdr); - struct in_addr dst_addr = ip4_hdr_get_dst_in_addr(hdr); - inet_ntop(AF_INET, &src_addr, src_addr_str, sizeof(src_addr_str)); - inet_ntop(AF_INET, &dst_addr, dst_addr_str, sizeof(dst_addr_str)); - - return snprintf(buf, size, "IPv4: version=%u hdr_len=%u tos=%u total_len=%u ipid=%u flags=%u(rf=%u df=%u mf=%u) frag_offset=%u ttl=%u proto=%s checksum=0x%x src_addr=%s dst_addr=%s opt_len=%u", - ip4_hdr_get_version(hdr), ip4_hdr_get_hdr_len(hdr), ip4_hdr_get_tos(hdr), - ip4_hdr_get_total_len(hdr), ip4_hdr_get_ipid(hdr), ip4_hdr_get_flags(hdr), - ip4_hdr_get_rf_flag(hdr), ip4_hdr_get_df_flag(hdr), ip4_hdr_get_mf_flag(hdr), - ip4_hdr_get_frag_offset(hdr), ip4_hdr_get_ttl(hdr), ip_proto_to_str(proto), - ip4_hdr_get_checksum(hdr), src_addr_str, dst_addr_str, ip4_hdr_get_opt_len(hdr)); -} - -#ifdef __cplusplus -} -#endif diff --git a/src/packet/ip6_utils.h b/src/packet/ip6_utils.h deleted file mode 100644 index e2af6c9..0000000 --- a/src/packet/ip6_utils.h +++ /dev/null @@ -1,221 +0,0 @@ -#pragma once - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include -#include -#include -#include - -/* - * IPv6 Header 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 - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * |Version| Traffic Class | Flow Label | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Payload Length | Next Header | Hop Limit | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | | - * + + - * | | - * + Source Address + - * | | - * + + - * | | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | | - * + + - * | | - * + Destination Address + - * | | - * + + - * | | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * - * Fragment Header - * - * 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 - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Next Header | Reserved | Fragment Offset |Res|M| - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Identification | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - */ - -/****************************************************************************** - * get - ******************************************************************************/ - -static inline uint8_t ip6_hdr_get_version(const struct ip6_hdr *hdr) -{ - return (ntohl(hdr->ip6_flow) & 0xf0000000) >> 28; -} - -static inline uint8_t ip6_hdr_get_traffic_class(const struct ip6_hdr *hdr) -{ - return (ntohl(hdr->ip6_flow) & 0x0ff00000) >> 20; -} - -static inline uint32_t ip6_hdr_get_flow_label(const struct ip6_hdr *hdr) -{ - return ntohl(hdr->ip6_flow) & 0x000fffff; -} - -static inline uint16_t ip6_hdr_get_payload_len(const struct ip6_hdr *hdr) -{ - return ntohs(hdr->ip6_plen); -} - -static inline uint8_t ip6_hdr_get_next_header(const struct ip6_hdr *hdr) -{ - return hdr->ip6_nxt; -} - -static inline uint8_t ip6_hdr_get_hop_limit(const struct ip6_hdr *hdr) -{ - return hdr->ip6_hlim; -} - -static inline struct in6_addr ip6_hdr_get_src_in6_addr(const struct ip6_hdr *hdr) -{ - return hdr->ip6_src; -} - -static inline struct in6_addr ip6_hdr_get_dst_in6_addr(const struct ip6_hdr *hdr) -{ - return hdr->ip6_dst; -} - -static inline struct ip6_frag *ip6_hdr_get_frag_ext(const struct ip6_hdr *hdr) -{ - if (hdr->ip6_nxt != IPPROTO_FRAGMENT) - { - return NULL; - } - return (struct ip6_frag *)((char *)hdr + sizeof(struct ip6_hdr)); -} - -/****************************************************************************** - * set - ******************************************************************************/ - -static inline void ip6_hdr_set_version(struct ip6_hdr *hdr, uint8_t version) -{ - hdr->ip6_flow = htonl((ntohl(hdr->ip6_flow) & 0x0fffffff) | (version << 28)); -} - -static inline void ip6_hdr_set_traffic_class(struct ip6_hdr *hdr, uint8_t traffic_class) -{ - hdr->ip6_flow = htonl((ntohl(hdr->ip6_flow) & 0xf00fffff) | (traffic_class << 20)); -} - -static inline void ip6_hdr_set_flow_label(struct ip6_hdr *hdr, uint32_t flow_label) -{ - hdr->ip6_flow = htonl((ntohl(hdr->ip6_flow) & 0xfff00000) | flow_label); -} - -static inline void ip6_hdr_set_payload_len(struct ip6_hdr *hdr, uint16_t payload_len) -{ - hdr->ip6_plen = htons(payload_len); -} - -static inline void ip6_hdr_set_next_header(struct ip6_hdr *hdr, uint8_t next_header) -{ - hdr->ip6_nxt = next_header; -} - -static inline void ip6_hdr_set_hop_limit(struct ip6_hdr *hdr, uint8_t hop_limit) -{ - hdr->ip6_hlim = hop_limit; -} - -static inline void ip6_hdr_set_src_in6_addr(struct ip6_hdr *hdr, struct in6_addr src_addr) -{ - hdr->ip6_src = src_addr; -} - -static inline void ip6_hdr_set_dst_in6_addr(struct ip6_hdr *hdr, struct in6_addr dst_addr) -{ - hdr->ip6_dst = dst_addr; -} - -/****************************************************************************** - * IPv6 frag extension headers - ******************************************************************************/ - -static inline uint8_t ipv6_frag_get_next_header(const struct ip6_frag *frag) -{ - return frag->ip6f_nxt; -} - -static inline uint16_t ipv6_frag_get_offset(const struct ip6_frag *frag) -{ - return ntohs(frag->ip6f_offlg & IP6F_OFF_MASK); -} - -static inline uint32_t ipv6_frag_get_ident(const struct ip6_frag *frag) -{ - return ntohl(frag->ip6f_ident); -} - -static inline bool ipv6_frag_get_more(const struct ip6_frag *frag) -{ - return (frag->ip6f_offlg & IP6F_MORE_FRAG); -} - -static inline void ipv6_frag_set_next_header(struct ip6_frag *frag, uint8_t next_header) -{ - frag->ip6f_nxt = next_header; -} - -static inline void ipv6_frag_set_offset(struct ip6_frag *frag, uint16_t offset) -{ - frag->ip6f_offlg = (frag->ip6f_offlg & ~IP6F_OFF_MASK) | htons(offset); -} - -static inline void ipv6_frag_set_ident(struct ip6_frag *frag, uint32_t ident) -{ - frag->ip6f_ident = htonl(ident); -} - -static inline void ipv6_frag_set_more(struct ip6_frag *frag, bool more) -{ - if (more) - { - frag->ip6f_offlg |= IP6F_MORE_FRAG; - } - else - { - frag->ip6f_offlg &= ~IP6F_MORE_FRAG; - } -} - -/****************************************************************************** - * print - ******************************************************************************/ - -static inline int ip6_hdr_to_str(const struct ip6_hdr *hdr, char *buf, size_t size) -{ - memset(buf, 0, size); - char src_addr_str[INET6_ADDRSTRLEN] = {0}; - char dst_addr_str[INET6_ADDRSTRLEN] = {0}; - - struct in6_addr src_addr = ip6_hdr_get_src_in6_addr(hdr); - struct in6_addr dst_addr = ip6_hdr_get_dst_in6_addr(hdr); - inet_ntop(AF_INET6, &src_addr, src_addr_str, INET6_ADDRSTRLEN); - inet_ntop(AF_INET6, &dst_addr, dst_addr_str, INET6_ADDRSTRLEN); - - return snprintf(buf, size, "IPv6: version=%u traffic_class=%u flow_label=%u payload_len=%u next_header=%u hop_limit=%u src_addr=%s dst_addr=%s", - ip6_hdr_get_version(hdr), ip6_hdr_get_traffic_class(hdr), ip6_hdr_get_flow_label(hdr), ip6_hdr_get_payload_len(hdr), - ip6_hdr_get_next_header(hdr), ip6_hdr_get_hop_limit(hdr), src_addr_str, dst_addr_str); -} - -#ifdef __cplusplus -} -#endif diff --git a/src/packet/l2tp_utils.h b/src/packet/l2tp_utils.h deleted file mode 100644 index 9d4ff97..0000000 --- a/src/packet/l2tp_utils.h +++ /dev/null @@ -1,261 +0,0 @@ -#pragma once - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include -#include -#include - -struct l2tp_hdr -{ - uint16_t flags; - // other option fields -}; - -#define L2TP_CONTROL_BIT 0x8000 -#define L2TP_LENGTH_BIT 0x4000 -#define L2TP_SEQUENCE_BIT 0x0800 -#define L2TP_OFFSET_BIT 0x0200 -#define L2TP_PRIORITY_BIT 0x0100 -#define L2TP_VERSION 0x000f - -/****************************************************************************** - * get - ******************************************************************************/ - -static inline uint8_t l2tp_hdr_get_ver(const struct l2tp_hdr *hdr) -{ - return ntohs(hdr->flags) & L2TP_VERSION; -} - -// 1: control message -// 0: data message -static inline uint8_t l2tp_hdr_get_type(const struct l2tp_hdr *hdr) -{ - return (ntohs(hdr->flags) & L2TP_CONTROL_BIT) >> 15; -} - -/* - * Layer Two Tunneling Protocol "L2TP" (V2) - * - * 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 - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * |T|L|x|x|S|x|O|P|x|x|x|x| Ver | Length (opt) | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Tunnel ID | Session ID | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Ns (opt) | Nr (opt) | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Offset Size (opt) | Offset pad... (opt) - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * - * https://datatracker.ietf.org/doc/html/rfc2661 - */ - -static inline uint16_t calc_udp_l2tpv2_hdr_len(const char *data, uint16_t len) -{ - const struct l2tp_hdr *hdr = (const struct l2tp_hdr *)data; - uint16_t flags = ntohs(hdr->flags); - - // ctrl message - if (flags & L2TP_CONTROL_BIT) - { - if ((flags & L2TP_LENGTH_BIT) == 0 || (flags & L2TP_SEQUENCE_BIT) == 0 || - (flags & L2TP_OFFSET_BIT) != 0 || (flags & L2TP_PRIORITY_BIT) != 0) - { - return 0; - } - else - { - return ntohs(*((uint16_t *)(data + 2))); - } - } - // data message - else - { - uint16_t skip_len = 2; // skip flags field - if (flags & L2TP_LENGTH_BIT) - { - skip_len += 2; // skip length field - } - skip_len += 2; // skip tunnel id field - skip_len += 2; // skip session id field - if (flags & L2TP_SEQUENCE_BIT) - { - skip_len += 2; // skip ns field - skip_len += 2; // skip nr field - } - if (flags & L2TP_OFFSET_BIT) - { - if (skip_len + 2 > len) - { - return 0; - } - uint16_t offset = ntohs(*((uint16_t *)(data + skip_len))); - if (offset == 0) - { - skip_len += 2; // skip offset field - } - else - { - skip_len = offset; - } - } - return skip_len; - } -} - -/* - * Figure 4.1.1.1: L2TPv3 Session Header Over IP - * - * 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 - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Session ID | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Cookie (optional, maximum 64 bits)... - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * - * Figure 4.1.1.2: L2TPv3 Control Message Header Over IP - * - * 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 - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | (32 bits of zeros) | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * |T|L|x|x|S|x|x|x|x|x|x|x| Ver | Length | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Control Connection ID | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Ns | Nr | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * - * Note: Unlike L2TP over UDP, which uses the T bit to distinguish between - * L2TP control and data packets, L2TP over IP uses the reserved Session - * ID of zero (0) when sending control messages. - * - * https://www.rfc-editor.org/rfc/rfc3931.html - */ - -static inline uint16_t calc_ip_l2tpv3_hdr_len(const char *data, uint16_t len) -{ - if (len < 4) - { - return 0; - } - uint32_t session_id = ntohl(*((uint32_t *)data)); - // data message - if (session_id) - { - // TODO The optional Cookie field contains a variable-length value (maximum 64 bits) - // TODO L2-Specific Sublayer 4 bytes - return 4 + 4; - } - // control message - else - { - if (len < 16) - { - return 0; - } - uint16_t flags = ntohs(*((uint16_t *)(data + 4))); - if ((flags & L2TP_LENGTH_BIT) == 0 || (flags & L2TP_SEQUENCE_BIT) == 0) - { - return 0; - } - else - { - return ntohs(*((uint16_t *)(data + 4 + 2))); - } - } -} - -/* - * Layer Two Tunneling Protocol - Version 3 (L2TPv3) - * - * Figure 3.2.1: L2TP Control Message Header - * - * 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 - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * |T|L|x|x|S|x|x|x|x|x|x|x| Ver | Length | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Control Connection ID | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Ns | Nr | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * - * Figure 4.1.2.1: L2TPv3 Session Header over UDP - * - * 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 - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * |T|x|x|x|x|x|x|x|x|x|x|x| Ver | Reserved | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Session ID | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Cookie (optional, maximum 64 bits)... - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * - * https://www.rfc-editor.org/rfc/rfc3931.html - */ - -static inline uint16_t calc_udp_l2tpv3_hdr_len(const char *data, uint16_t len) -{ - if (len < 8) - { - return 0; - } - - const struct l2tp_hdr *hdr = (const struct l2tp_hdr *)data; - uint16_t flags = ntohs(hdr->flags); - - // ctrl message - if (flags & L2TP_CONTROL_BIT) - { - if ((flags & L2TP_LENGTH_BIT) == 0 || (flags & L2TP_SEQUENCE_BIT) == 0) - { - return 0; - } - else - { - return ntohs(*((uint16_t *)(data + 2))); - } - } - // data message - else - { - // TODO The optional Cookie field contains a variable-length value (maximum 64 bits) - // TODO L2-Specific Sublayer 4 bytes - return 8 + 4; - } -} - -/****************************************************************************** - * set - ******************************************************************************/ - -// TODO - -/****************************************************************************** - * print - ******************************************************************************/ - -static inline int l2tp_hdr_to_str(const struct l2tp_hdr *hdr, char *buf, size_t size) -{ - memset(buf, 0, size); - return snprintf(buf, size, "L2TP: type=%s version=%u", - l2tp_hdr_get_type(hdr) ? "control" : "data", l2tp_hdr_get_ver(hdr)); -} - -#ifdef __cplusplus -} -#endif diff --git a/src/packet/mpls_utils.h b/src/packet/mpls_utils.h deleted file mode 100644 index e0b91e9..0000000 --- a/src/packet/mpls_utils.h +++ /dev/null @@ -1,84 +0,0 @@ -#pragma once - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include -#include -#include -#include - -/* - * 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 | TC |S| TTL | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - */ - -/****************************************************************************** - * get - ******************************************************************************/ - -static inline uint32_t mpls_label_get_label(const struct mpls_label *hdr) -{ - return ((ntohl(hdr->entry) & MPLS_LS_LABEL_MASK) >> MPLS_LS_LABEL_SHIFT); -} - -static inline uint8_t mpls_label_get_tc(const struct mpls_label *hdr) -{ - return ((ntohl(hdr->entry) & MPLS_LS_TC_MASK) >> MPLS_LS_TC_SHIFT); -} - -static inline uint8_t mpls_label_get_bos(const struct mpls_label *hdr) -{ - return ((ntohl(hdr->entry) & MPLS_LS_S_MASK) >> MPLS_LS_S_SHIFT); -} - -static inline uint8_t mpls_label_get_ttl(const struct mpls_label *hdr) -{ - return ((ntohl(hdr->entry) & MPLS_LS_TTL_MASK) >> MPLS_LS_TTL_SHIFT); -} - -/****************************************************************************** - * set - ******************************************************************************/ - -static inline void mpls_label_set_label(struct mpls_label *hdr, uint32_t label) -{ - hdr->entry = htonl((ntohl(hdr->entry) & ~MPLS_LS_LABEL_MASK) | (label << MPLS_LS_LABEL_SHIFT)); -} - -static inline void mpls_label_set_tc(struct mpls_label *hdr, uint8_t tc) -{ - hdr->entry = htonl((ntohl(hdr->entry) & ~MPLS_LS_TC_MASK) | (tc << MPLS_LS_TC_SHIFT)); -} - -static inline void mpls_label_set_bos(struct mpls_label *hdr, uint8_t bos) -{ - hdr->entry = htonl((ntohl(hdr->entry) & ~MPLS_LS_S_MASK) | (bos << MPLS_LS_S_SHIFT)); -} - -static inline void mpls_label_set_ttl(struct mpls_label *hdr, uint8_t ttl) -{ - hdr->entry = htonl((ntohl(hdr->entry) & ~MPLS_LS_TTL_MASK) | (ttl << MPLS_LS_TTL_SHIFT)); -} - -/****************************************************************************** - * print - ******************************************************************************/ - -static inline int mpls_label_to_str(const struct mpls_label *hdr, char *buf, size_t size) -{ - return snprintf(buf, size, "MPLS: label=%u tc=%u bos=%u ttl=%u", - mpls_label_get_label(hdr), mpls_label_get_tc(hdr), - mpls_label_get_bos(hdr), mpls_label_get_ttl(hdr)); -} - -#ifdef __cplusplus -} -#endif diff --git a/src/packet/packet_builder.cpp b/src/packet/packet_builder.cpp index dd49d7a..ad10192 100644 --- a/src/packet/packet_builder.cpp +++ b/src/packet/packet_builder.cpp @@ -2,13 +2,8 @@ #include "log.h" #include "checksum.h" -#include "tcp_utils.h" -#include "udp_utils.h" -#include "ip4_utils.h" -#include "ip6_utils.h" -#include "gtp_utils.h" -#include "gre_utils.h" #include "packet_private.h" +#include "packet_helper.h" #include "packet_utils.h" #include "packet_layer.h" #include "packet_parser.h" diff --git a/src/packet/packet_dump.cpp b/src/packet/packet_dump.cpp index a08d012..be3ca67 100644 --- a/src/packet/packet_dump.cpp +++ b/src/packet/packet_dump.cpp @@ -6,17 +6,7 @@ #include "log.h" #include "utils.h" -#include "eth_utils.h" -#include "gre_utils.h" -#include "udp_utils.h" -#include "tcp_utils.h" -#include "ip4_utils.h" -#include "ip6_utils.h" -#include "gtp_utils.h" -#include "mpls_utils.h" -#include "l2tp_utils.h" -#include "vlan_utils.h" -#include "vxlan_utils.h" +#include "packet_helper.h" #include "packet_private.h" #include "packet_dump.h" #include "packet_parser.h" diff --git a/src/packet/packet_helper.h b/src/packet/packet_helper.h new file mode 100644 index 0000000..f4beda2 --- /dev/null +++ b/src/packet/packet_helper.h @@ -0,0 +1,2870 @@ +#pragma once + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "utils.h" +#include +#include +#include +#include +#include +#include +#include +#define __FAVOR_BSD 1 +#include +#include + +/****************************************************************************** + * ETH + ******************************************************************************/ + +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); +} + +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); +} + +// /usr/include/linux/if_ether.h +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_8021AD: + 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: + case 0x880B: // PPP：Point-to-Point Protoco + 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_8021AD: + return "ETH_P_8021AD"; + 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"; + case 0x880B: + return "ETH_P_PPP"; + default: + return "ETH_P_UNKNOWN"; + } +} + +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)); +} + +/****************************************************************************** + * GREv0 + ******************************************************************************/ + +/* + * 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 + */ + +struct gre0_hdr +{ + uint16_t flags; + uint16_t protocol; +}; + +struct sre +{ + uint16_t family; + uint8_t offset; + uint8_t length; +}; + +#define GRE0_FLAG_CHECKSUM 0x8000 +#define GRE0_FLAG_ROUTING 0x4000 +#define GRE0_FLAG_KEY 0x2000 +#define GRE0_FLAG_SEQUENCE 0x1000 +#define GRE0_FLAG_STRICT 0x0800 +#define GRE0_FLAG_RECURSION 0x0700 +#define GRE0_FLAG_VERSION 0x0007 + +static inline uint16_t gre0_hdr_get_flags(const struct gre0_hdr *hdr) +{ + return ntohs(hdr->flags); +} + +static inline uint8_t gre0_hdr_get_checksum_flag(const struct gre0_hdr *hdr) +{ + return (ntohs(hdr->flags) & GRE0_FLAG_CHECKSUM) >> 15; +} + +static inline uint8_t gre0_hdr_get_routing_flag(const struct gre0_hdr *hdr) +{ + return (ntohs(hdr->flags) & GRE0_FLAG_ROUTING) >> 14; +} + +static inline uint8_t gre0_hdr_get_key_flag(const struct gre0_hdr *hdr) +{ + return (ntohs(hdr->flags) & GRE0_FLAG_KEY) >> 13; +} + +static inline uint8_t gre0_hdr_get_seq_flag(const struct gre0_hdr *hdr) +{ + return (ntohs(hdr->flags) & GRE0_FLAG_SEQUENCE) >> 12; +} + +static inline uint8_t gre0_hdr_get_strict_flag(const struct gre0_hdr *hdr) +{ + return (ntohs(hdr->flags) & GRE0_FLAG_STRICT) >> 11; +} + +static inline uint8_t gre0_hdr_get_version(const struct gre0_hdr *hdr) +{ + return ntohs(hdr->flags) & GRE0_FLAG_VERSION; +} + +// ethernet protocol +static inline uint16_t gre0_hdr_get_proto(const struct gre0_hdr *hdr) +{ + return ntohs(hdr->protocol); +} + +static inline uint16_t gre0_hdr_get_checksum(const struct gre0_hdr *hdr) +{ + if (gre0_hdr_get_checksum_flag(hdr)) + { + const char *ptr = ((const char *)hdr) + 4; + return ntohs(*(uint16_t *)ptr); + } + else + { + return 0; + } +} + +static inline uint16_t gre0_hdr_get_offset(const struct gre0_hdr *hdr) +{ + if (gre0_hdr_get_checksum_flag(hdr)) + { + const char *ptr = ((const char *)hdr) + 6; + return ntohs(*(uint16_t *)ptr); + } + else + { + return 0; + } +} + +static inline uint32_t gre0_hdr_get_key(const struct gre0_hdr *hdr) +{ + if (gre0_hdr_get_key_flag(hdr)) + { + const char *ptr = ((const char *)hdr) + 4; + if (gre0_hdr_get_checksum_flag(hdr)) + { + ptr += 4; + } + + return ntohl(*(uint32_t *)ptr); + } + else + { + return 0; + } +} + +static inline uint32_t gre0_hdr_get_seq(const struct gre0_hdr *hdr) +{ + if (gre0_hdr_get_seq_flag(hdr)) + { + const char *ptr = ((const char *)hdr) + 4; + if (gre0_hdr_get_checksum_flag(hdr)) + { + ptr += 4; + } + + if (gre0_hdr_get_key_flag(hdr)) + { + ptr += 4; + } + + return ntohl(*(uint32_t *)ptr); + } + else + { + return 0; + } +} + +static inline const char *gre0_hdr_get_routing_data(const struct gre0_hdr *hdr) +{ + if (gre0_hdr_get_routing_flag(hdr)) + { + const char *ptr = ((const char *)hdr) + 4; + if (gre0_hdr_get_checksum_flag(hdr)) + { + ptr += 4; + } + + if (gre0_hdr_get_key_flag(hdr)) + { + ptr += 4; + } + + if (gre0_hdr_get_seq_flag(hdr)) + { + ptr += 4; + } + + return ptr; + } + else + { + return NULL; + } +} + +static inline uint16_t gre0_hdr_get_routing_len(const struct gre0_hdr *hdr) +{ + if (gre0_hdr_get_routing_flag(hdr)) + { + const char *ptr = gre0_hdr_get_routing_data(hdr); + + uint16_t hdr_len = 0; + while (1) + { + struct sre *sre = (struct sre *)(ptr + hdr_len); + if (sre->length == 0) + { + hdr_len += sizeof(struct sre); + break; + } + else + { + hdr_len += sizeof(struct sre) + sre->length; + } + } + return hdr_len; + } + else + { + return 0; + } +} + +static inline uint16_t calc_gre0_hdr_len(const char *data, uint32_t len) +{ + if (data == NULL || len < sizeof(struct gre0_hdr)) + { + return 0; + } + const struct gre0_hdr *hdr = (const struct gre0_hdr *)data; + uint16_t hdr_len = 4; + uint16_t flags = ntohs(hdr->flags); + if ((flags & GRE0_FLAG_CHECKSUM) || (flags & GRE0_FLAG_ROUTING)) + { + hdr_len += 4; // skip checksum and offset fields + } + if (flags & GRE0_FLAG_KEY) + { + hdr_len += 4; // skip key field + } + if (flags & GRE0_FLAG_SEQUENCE) + { + hdr_len += 4; // skip sequence number field + } + if (flags & GRE0_FLAG_ROUTING) + { + while (hdr_len + sizeof(struct sre) <= len) + { + struct sre *sre = (struct sre *)((char *)data + hdr_len); + if (sre->length == 0) + { + hdr_len += sizeof(struct sre); + break; + } + else + { + hdr_len += sizeof(struct sre) + sre->length; + } + } + } + return hdr_len; +} + +static inline void gre0_hdr_set_flags(struct gre0_hdr *hdr, uint16_t flags) +{ + hdr->flags = htons(flags); +} + +static inline void gre0_hdr_set_checksum_flag(struct gre0_hdr *hdr, uint8_t flag) +{ + hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_CHECKSUM) | flag << 15); +} + +static inline void gre0_hdr_set_routing_flag(struct gre0_hdr *hdr, uint8_t flag) +{ + hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_ROUTING) | flag << 14); +} + +static inline void gre0_hdr_set_key_flag(struct gre0_hdr *hdr, uint8_t flag) +{ + hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_KEY) | flag << 13); +} + +static inline void gre0_hdr_set_seq_flag(struct gre0_hdr *hdr, uint8_t flag) +{ + hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_SEQUENCE) | flag << 12); +} + +static inline void gre0_hdr_set_strict_flag(struct gre0_hdr *hdr, uint8_t flag) +{ + hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_STRICT) | flag << 11); +} + +static inline void gre0_hdr_set_version(struct gre0_hdr *hdr, uint8_t version) +{ + hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_VERSION) | version); +} + +static inline void gre0_hdr_set_proto(struct gre0_hdr *hdr, uint16_t proto) +{ + hdr->protocol = htons(proto); +} + +static inline void gre0_hdr_set_checksum(struct gre0_hdr *hdr, uint16_t checksum) +{ + if (gre0_hdr_get_checksum_flag(hdr)) + { + char *ptr = ((char *)hdr) + 4; + *(uint16_t *)ptr = htons(checksum); + } +} + +static inline void gre0_hdr_set_offset(struct gre0_hdr *hdr, uint16_t offset) +{ + if (gre0_hdr_get_checksum_flag(hdr)) + { + char *ptr = ((char *)hdr) + 6; + *(uint16_t *)ptr = htons(offset); + } +} + +static inline void gre0_hdr_set_key(struct gre0_hdr *hdr, uint32_t key) +{ + if (gre0_hdr_get_key_flag(hdr)) + { + char *ptr = ((char *)hdr) + 4; + if (gre0_hdr_get_checksum_flag(hdr)) + { + ptr += 4; + } + + *(uint32_t *)ptr = htonl(key); + } +} + +static inline void gre0_hdr_set_seq(struct gre0_hdr *hdr, uint32_t sequence) +{ + if (gre0_hdr_get_seq_flag(hdr)) + { + char *ptr = ((char *)hdr) + 4; + if (gre0_hdr_get_checksum_flag(hdr)) + { + ptr += 4; + } + + if (gre0_hdr_get_key_flag(hdr)) + { + ptr += 4; + } + + *(uint32_t *)ptr = htonl(sequence); + } +} + +static inline void gre0_hdr_set_routing_data(struct gre0_hdr *hdr, const char *data, uint16_t len) +{ + if (gre0_hdr_get_routing_flag(hdr)) + { + char *ptr = ((char *)hdr) + 4; + if (gre0_hdr_get_checksum_flag(hdr)) + { + ptr += 4; + } + + if (gre0_hdr_get_key_flag(hdr)) + { + ptr += 4; + } + + if (gre0_hdr_get_seq_flag(hdr)) + { + ptr += 4; + } + + memcpy(ptr, data, len); + } +} + +static inline int gre0_hdr_to_str(const struct gre0_hdr *hdr, char *buf, size_t size) +{ + memset(buf, 0, size); + + int used = 0; + uint16_t proto = gre0_hdr_get_proto(hdr); + used += snprintf(buf + used, size - used, "GRE: flags=0x%04x (checksum_flag=%u, routing_flag=%u, key_flag=%u, seq_flag=%u, strict_flag=%u, version=%u), proto=%s", + gre0_hdr_get_flags(hdr), gre0_hdr_get_checksum_flag(hdr), gre0_hdr_get_routing_flag(hdr), gre0_hdr_get_key_flag(hdr), + gre0_hdr_get_seq_flag(hdr), gre0_hdr_get_strict_flag(hdr), gre0_hdr_get_version(hdr), eth_proto_to_str(proto)); + if (gre0_hdr_get_checksum_flag(hdr)) + { + used += snprintf(buf + used, size - used, ", checksum=0x%x, offset=%u", gre0_hdr_get_checksum(hdr), gre0_hdr_get_offset(hdr)); + } + if (gre0_hdr_get_key_flag(hdr)) + { + used += snprintf(buf + used, size - used, ", key=%u", gre0_hdr_get_key(hdr)); + } + if (gre0_hdr_get_seq_flag(hdr)) + { + used += snprintf(buf + used, size - used, ", seq=%u", gre0_hdr_get_seq(hdr)); + } + if (gre0_hdr_get_routing_flag(hdr)) + { + used += snprintf(buf + used, size - used, ", routing_len=%u", gre0_hdr_get_routing_len(hdr)); + } + + return used; +} + +/****************************************************************************** + * GREv1 + ******************************************************************************/ + +/* + * 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 + */ + +struct gre1_hdr +{ + uint16_t flags; + uint16_t protocol; + uint16_t payload_length; + uint16_t call_id; +}; + +#define GRE1_FLAG_CHECKSUM 0x8000 +#define GRE1_FLAG_ROUTING 0x4000 +#define GRE1_FLAG_KEY 0x2000 +#define GRE1_FLAG_SEQUENCE 0x1000 +#define GRE1_FLAG_STRICT 0x0800 +#define GRE1_FLAG_RECURSION 0x0700 +#define GRE1_FLAG_ACK 0x0080 /* only in special PPTPized GRE header */ +#define GRE1_FLAG_VERSION 0x0007 + +static inline uint16_t gre1_hdr_get_flags(const struct gre1_hdr *hdr) +{ + return ntohs(hdr->flags); +} + +static inline uint8_t gre1_hdr_get_seq_flag(const struct gre1_hdr *hdr) +{ + return (ntohs(hdr->flags) & GRE1_FLAG_SEQUENCE) >> 12; +} + +static inline uint8_t gre1_hdr_get_ack_flag(const struct gre1_hdr *hdr) +{ + return (ntohs(hdr->flags) & GRE1_FLAG_ACK) >> 7; +} + +static inline uint8_t gre1_hdr_get_version(const struct gre1_hdr *hdr) +{ + return ntohs(hdr->flags) & GRE1_FLAG_VERSION; +} + +// ethernet protocol type +static inline uint16_t gre1_hdr_get_proto(const struct gre1_hdr *hdr) +{ + return ntohs(hdr->protocol); +} + +static inline uint16_t gre1_hdr_get_payload_length(const struct gre1_hdr *hdr) +{ + return ntohs(hdr->payload_length); +} + +static inline uint16_t gre1_hdr_get_call_id(const struct gre1_hdr *hdr) +{ + return ntohs(hdr->call_id); +} + +static inline uint32_t gre1_hdr_get_seq(const struct gre1_hdr *hdr) +{ + if (gre1_hdr_get_seq_flag(hdr)) + { + const char *ptr = ((const char *)hdr) + 8; + return ntohl(*((uint32_t *)ptr)); + } + else + { + return 0; + } +} + +static inline uint32_t gre1_hdr_get_ack(const struct gre1_hdr *hdr) +{ + if (gre1_hdr_get_ack_flag(hdr)) + { + const char *ptr = ((const char *)hdr) + 8; + if (gre1_hdr_get_seq_flag(hdr)) + { + ptr += 4; + } + return ntohl(*((uint32_t *)ptr)); + } + else + { + return 0; + } +} + +static inline uint16_t calc_gre1_hdr_len(const char *data, uint32_t len) +{ + if (data == NULL || len < sizeof(struct gre1_hdr)) + { + return 0; + } + const struct gre1_hdr *hdr = (const struct gre1_hdr *)data; + uint16_t hdr_len = 8; + uint16_t flags = gre1_hdr_get_flags(hdr); + if (flags & GRE1_FLAG_SEQUENCE) + { + hdr_len += 4; + } + if (flags & GRE1_FLAG_ACK) + { + hdr_len += 4; + } + return hdr_len; +} + +static inline void gre1_hdr_set_flags(struct gre1_hdr *hdr, uint16_t flags) +{ + hdr->flags = htons(flags); +} + +static inline void gre1_hdr_set_seq_flag(struct gre1_hdr *hdr, uint8_t seq_flag) +{ + hdr->flags = htons((ntohs(hdr->flags) & ~GRE1_FLAG_SEQUENCE) | seq_flag << 12); +} + +static inline void gre1_hdr_set_ack_flag(struct gre1_hdr *hdr, uint8_t ack_flag) +{ + hdr->flags = htons((ntohs(hdr->flags) & ~GRE1_FLAG_ACK) | ack_flag << 7); +} + +static inline void gre1_hdr_set_version(struct gre1_hdr *hdr, uint8_t version) +{ + hdr->flags = htons((ntohs(hdr->flags) & ~GRE1_FLAG_VERSION) | version); +} + +static inline void gre1_hdr_set_proto(struct gre1_hdr *hdr, uint16_t proto) +{ + hdr->protocol = htons(proto); +} + +static inline void gre1_hdr_set_payload_length(struct gre1_hdr *hdr, uint16_t payload_length) +{ + hdr->payload_length = htons(payload_length); +} + +static inline void gre1_hdr_set_call_id(struct gre1_hdr *hdr, uint16_t call_id) +{ + hdr->call_id = htons(call_id); +} + +static inline void gre1_hdr_set_seq(struct gre1_hdr *hdr, uint32_t seq) +{ + if (gre1_hdr_get_seq_flag(hdr)) + { + char *ptr = ((char *)hdr) + 8; + *((uint32_t *)ptr) = htonl(seq); + } +} + +static inline void gre1_hdr_set_ack(struct gre1_hdr *hdr, uint32_t ack) +{ + if (gre1_hdr_get_ack_flag(hdr)) + { + char *ptr = ((char *)hdr) + 8; + if (gre1_hdr_get_seq_flag(hdr)) + { + ptr += 4; + } + *((uint32_t *)ptr) = htonl(ack); + } +} + +static inline int gre1_hdr_to_str(const struct gre1_hdr *hdr, char *buf, size_t size) +{ + memset(buf, 0, size); + + int used = 0; + uint16_t proto = gre1_hdr_get_proto(hdr); + used += snprintf(buf + used, size - used, "GRE: flags=0x%04x (seq_flag=%u ack_flag=%u version=%u), proto=%s, payload_length=%u, call_id=%u", + gre1_hdr_get_flags(hdr), gre1_hdr_get_seq_flag(hdr), gre1_hdr_get_ack_flag(hdr), gre1_hdr_get_version(hdr), + eth_proto_to_str(proto), gre1_hdr_get_payload_length(hdr), gre1_hdr_get_call_id(hdr)); + if (gre1_hdr_get_seq_flag(hdr)) + { + used += snprintf(buf + used, size - used, ", seq=%u", gre1_hdr_get_seq(hdr)); + } + if (gre1_hdr_get_ack_flag(hdr)) + { + used += snprintf(buf + used, size - used, ", ack=%u", gre1_hdr_get_ack(hdr)); + } + + return used; +} + +/****************************************************************************** + * GRE + ******************************************************************************/ + +// return GRE version 0 or 1 +// return 255 if not GRE +static inline uint8_t peek_gre_version(const char *data, uint32_t len) +{ + if (len < MIN(sizeof(struct gre0_hdr), sizeof(struct gre1_hdr))) + { + return 255; + } + + uint16_t flag = *(uint16_t *)data; + return ntohs(flag) & 0x0007; +} + +static inline uint16_t peek_gre_proto(const char *data, uint32_t len) +{ + switch (peek_gre_version(data, len)) + { + case 0: + return gre0_hdr_get_proto((const struct gre0_hdr *)data); + case 1: + return gre1_hdr_get_proto((const struct gre1_hdr *)data); + default: + return 0; + } +} + +static inline uint16_t calc_gre_hdr_len(const char *data, uint32_t len) +{ + switch (peek_gre_version(data, len)) + { + case 0: + return calc_gre0_hdr_len(data, len); + case 1: + return calc_gre1_hdr_len(data, len); + default: + return 0; + } +} + +static inline int gre_hdr_to_str(const char *hdr, uint16_t len, char *buf, size_t size) +{ + switch (peek_gre_version(hdr, len)) + { + case 0: + return gre0_hdr_to_str((const struct gre0_hdr *)hdr, buf, size); + case 1: + return gre1_hdr_to_str((const struct gre1_hdr *)hdr, buf, size); + default: + return 0; + } +} + +/****************************************************************************** + * GTPv1 + ******************************************************************************/ + +/* + * https://en.wikipedia.org/wiki/GPRS_Tunnelling_Protocol + * + * GTP-C: Gateway GPRS Support Nodes (GGSN) <----> Serving GPRS support Nodes (SGSN) + * GTP-U: Radio Access Network <----> Core Network + * + * GTPv1: + * -> GTPv1-C + * -> GTPv1-U + */ + +/* + * GTP 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 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Ver |T|R|E|S|N| Message Type | Message Length | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | TEID | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Sequence Number(O) |N-PDU Number(O)|Next Ext Hdr(O)| (optional headers) + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * + * Message Length: + * a 16-bit field that indicates the length of the payload in bytes + * (rest of the packet following the mandatory 8-byte GTP header). + * Includes the optional fields. + */ + +/* + * Next Extension Headers + * + * 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 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Length | Contents | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | ... | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Contents | Next Ext Hdr | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * + * Extension length + * an 8-bit field. This field states the length of this extension header, + * including the length, the contents, and the next extension header field, + * in 4-octet units, so the length of the extension must always be a multiple of 4. + */ + +struct gtp1_hdr +{ + uint8_t flags; + uint8_t msg_type; + uint16_t msg_len; + uint32_t teid; +} __attribute__((packed)); + +struct gtp1_hdr_long +{ + uint8_t flags; + uint8_t msg_type; + uint16_t msg_len; + uint32_t teid; + + uint16_t seq; + uint8_t npdu; + uint8_t next_ext_hdr; +} __attribute__((packed)); + +#define GTP1_FLAG_N_PDU (0x01) +#define GTP1_FLAG_SEQ_NUM (0x02) +#define GTP1_FLAG_EXT_HDR (0x04) +#define GTP1_FLAG_RESERVED (0x08) +#define GTP1_FLAG_PROTOCOL (0x10) +#define GTP1_FLAG_VERSION (0xE0) + +static inline uint8_t gtp1_hdr_get_flags(const struct gtp1_hdr *gtp) +{ + return gtp->flags; +} + +static inline uint8_t gtp1_hdr_get_version(const struct gtp1_hdr *gtp) +{ + return (gtp->flags & GTP1_FLAG_VERSION) >> 5; +} + +static inline uint8_t gtp1_hdr_get_proto(const struct gtp1_hdr *gtp) +{ + return (gtp->flags & GTP1_FLAG_PROTOCOL) >> 4; +} + +static inline uint8_t gtp1_hdr_get_reserved(const struct gtp1_hdr *gtp) +{ + return (gtp->flags & GTP1_FLAG_RESERVED) >> 3; +} + +static inline uint8_t gtp1_hdr_get_ext_flag(const struct gtp1_hdr *gtp) +{ + return (gtp->flags & GTP1_FLAG_EXT_HDR) >> 2; +} + +static inline uint8_t gtp1_hdr_get_seq_flag(const struct gtp1_hdr *gtp) +{ + return (gtp->flags & GTP1_FLAG_SEQ_NUM) >> 1; +} + +static inline uint8_t gtp1_hdr_get_npdu_flag(const struct gtp1_hdr *gtp) +{ + return (gtp->flags & GTP1_FLAG_N_PDU) >> 0; +} + +static inline uint8_t gtp1_hdr_get_msg_type(const struct gtp1_hdr *gtp) +{ + return gtp->msg_type; +} + +static inline uint16_t gtp1_hdr_get_msg_len(const struct gtp1_hdr *gtp) +{ + return ntohs(gtp->msg_len); +} + +static inline uint32_t gtp1_hdr_get_teid(const struct gtp1_hdr *gtp) +{ + return ntohl(gtp->teid); +} + +static inline uint16_t gtp1_hdr_get_seq(const struct gtp1_hdr *gtp) +{ + if (gtp1_hdr_get_seq_flag(gtp)) + { + const struct gtp1_hdr_long *gtp_long = (const struct gtp1_hdr_long *)gtp; + return ntohs(gtp_long->seq); + } + else + { + return 0; + } +} + +static inline uint8_t gtp1_hdr_get_npdu(const struct gtp1_hdr *gtp) +{ + if (gtp1_hdr_get_npdu_flag(gtp)) + { + const struct gtp1_hdr_long *gtp_long = (const struct gtp1_hdr_long *)gtp; + return gtp_long->npdu; + } + else + { + return 0; + } +} + +static inline uint8_t gtp1_hdr_get_next_ext_type(const struct gtp1_hdr *gtp) +{ + if (gtp1_hdr_get_ext_flag(gtp)) + { + const struct gtp1_hdr_long *gtp_long = (const struct gtp1_hdr_long *)gtp; + return gtp_long->next_ext_hdr; + } + else + { + return 0; + } +} + +// include gtp fixed header and optional headers and extension headers +static inline uint16_t calc_gtp1_hdr_len(const char *data, uint16_t len) +{ + if (data == NULL || len < sizeof(struct gtp1_hdr)) + { + return 0; + } + + const struct gtp1_hdr *gtp = (const struct gtp1_hdr *)data; + if (gtp1_hdr_get_version(gtp) != 1) + { + return 0; + } + + if (gtp1_hdr_get_flags(gtp) & (GTP1_FLAG_SEQ_NUM | GTP1_FLAG_N_PDU | GTP1_FLAG_EXT_HDR)) + { + if (sizeof(struct gtp1_hdr_long) > len) + { + return 0; + } + + const struct gtp1_hdr_long *gtp_long = (const struct gtp1_hdr_long *)data; + uint8_t next_ext_hdr = gtp_long->next_ext_hdr; + uint16_t offset = sizeof(struct gtp1_hdr_long); + + while (next_ext_hdr) + { + if (offset + 1 > len) + { + return 0; + } + uint16_t ext_hdr_len = *((char *)data + offset) * 4; + if (ext_hdr_len == 0 || offset + ext_hdr_len > len) + { + return 0; + } + offset += ext_hdr_len; // skip extension header + next_ext_hdr = *((char *)data + offset - 1); + } + + return offset; + } + else + { + return sizeof(struct gtp1_hdr); + } +} + +static inline void gtp1_hdr_set_flags(struct gtp1_hdr *gtp, uint8_t flags) +{ + gtp->flags = flags; +} + +static inline void gtp1_hdr_set_version(struct gtp1_hdr *gtp, uint8_t version) +{ + gtp->flags = (gtp->flags & ~GTP1_FLAG_VERSION) | (version << 5); +} + +static inline void gtp1_hdr_set_proto(struct gtp1_hdr *gtp, uint8_t protocol_type) +{ + gtp->flags = (gtp->flags & ~GTP1_FLAG_PROTOCOL) | (protocol_type << 4); +} + +static inline void gtp1_hdr_set_reserved(struct gtp1_hdr *gtp, uint8_t reserved) +{ + gtp->flags = (gtp->flags & ~GTP1_FLAG_RESERVED) | (reserved << 3); +} + +static inline void gtp1_hdr_set_ext_flag(struct gtp1_hdr *gtp, uint8_t ext_flag) +{ + gtp->flags = (gtp->flags & ~GTP1_FLAG_EXT_HDR) | (ext_flag << 2); +} + +static inline void gtp1_hdr_set_seq_flag(struct gtp1_hdr *gtp, uint8_t seq_flag) +{ + gtp->flags = (gtp->flags & ~GTP1_FLAG_SEQ_NUM) | (seq_flag << 1); +} + +static inline void gtp1_hdr_set_npdu_flag(struct gtp1_hdr *gtp, uint8_t npdu_flag) +{ + gtp->flags = (gtp->flags & ~GTP1_FLAG_N_PDU) | (npdu_flag << 0); +} + +static inline void gtp1_hdr_set_msg_type(struct gtp1_hdr *gtp, uint8_t msg_type) +{ + gtp->msg_type = msg_type; +} + +static inline void gtp1_hdr_set_msg_len(struct gtp1_hdr *gtp, uint16_t msg_len) +{ + gtp->msg_len = htons(msg_len); +} + +static inline void gtp1_hdr_set_teid(struct gtp1_hdr *gtp, uint32_t teid) +{ + gtp->teid = htonl(teid); +} + +static inline void gtp1_hdr_set_seq(struct gtp1_hdr *gtp, uint16_t seq) +{ + if (gtp1_hdr_get_seq_flag(gtp)) + { + struct gtp1_hdr_long *gtp_long = (struct gtp1_hdr_long *)gtp; + gtp_long->seq = htons(seq); + } +} + +static inline void gtp1_hdr_set_npdu(struct gtp1_hdr *gtp, uint8_t npdu) +{ + if (gtp1_hdr_get_npdu_flag(gtp)) + { + struct gtp1_hdr_long *gtp_long = (struct gtp1_hdr_long *)gtp; + gtp_long->npdu = npdu; + } +} + +static inline void gtp1_hdr_set_next_ext_type(struct gtp1_hdr *gtp, uint8_t next_ext_hdr) +{ + if (gtp1_hdr_get_ext_flag(gtp)) + { + struct gtp1_hdr_long *gtp_long = (struct gtp1_hdr_long *)gtp; + gtp_long->next_ext_hdr = next_ext_hdr; + } +} + +static inline int gtp1_hdr_to_str(const struct gtp1_hdr *gtp, char *buf, size_t len) +{ + memset(buf, 0, len); + + int used = 0; + used += snprintf(buf + used, len - used, "GTP: flags=0x%02x (version=%u, protocol=%u, reserved=%u, ext_flag=%u, seq_flag=%u, npdu_flag=%u), msg_type=0x%02x, msg_len=%u, teid=%u", + gtp1_hdr_get_flags(gtp), + gtp1_hdr_get_version(gtp), + gtp1_hdr_get_proto(gtp), + gtp1_hdr_get_reserved(gtp), + gtp1_hdr_get_ext_flag(gtp), + gtp1_hdr_get_seq_flag(gtp), + gtp1_hdr_get_npdu_flag(gtp), + gtp1_hdr_get_msg_type(gtp), + gtp1_hdr_get_msg_len(gtp), + gtp1_hdr_get_teid(gtp)); + + if (gtp1_hdr_get_seq_flag(gtp)) + { + used += snprintf(buf + used, len - used, ", seq=%u", gtp1_hdr_get_seq(gtp)); + } + if (gtp1_hdr_get_npdu_flag(gtp)) + { + used += snprintf(buf + used, len - used, ", npdu=%u", gtp1_hdr_get_npdu(gtp)); + } + if (gtp1_hdr_get_ext_flag(gtp)) + { + used += snprintf(buf + used, len - used, ", next_ext_hdr=%u", gtp1_hdr_get_next_ext_type(gtp)); + } + return used; +} + +/****************************************************************************** + * GTPv2 + ******************************************************************************/ + +/* + * https://en.wikipedia.org/wiki/GPRS_Tunnelling_Protocol + * https://www.etsi.org/deliver/etsi_ts/129200_129299/129274/08.04.00_60/ts_129274v080400p.pdf + * + * GTP-C: Gateway GPRS Support Nodes (GGSN) <----> Serving GPRS support Nodes (SGSN) + * GTP-U: Radio Access Network <----> Core Network + * + * GTPv2: + * -> GTPv2-C + * -> (no GTPv2-U) + */ + +/* + * GTP version 2 + * + * 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 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Ver |P|T|spare| Message Type | Message Length | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | TEID (only if T=1) | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Sequence Number | Spare | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * + * Message length: + * This field shall indicate the length of the message in octets excluding the mandatory of the GTP-C header (the first 4 octets). + * The TEID (if present) and the Sequence Number shall be included in the length count. + */ + +struct gtp2_hdr +{ + uint8_t flags; + uint8_t msg_type; + uint16_t msg_len; + uint32_t seq_and_spare; +} __attribute__((packed)); + +struct gtp2_hdr_long +{ + uint8_t flags; + uint8_t msg_type; + uint16_t msg_len; + uint32_t teid; + uint32_t seq_and_spare; +} __attribute__((packed)); + +#define GPT2_FLAG_SPARE (0x07) +#define GTP2_FLAG_TEID (0x08) +#define GTP2_FLAG_PIGGYBACK (0x10) +#define GTP2_FLAG_VERSION (0xE0) + +static inline uint8_t gtp2_hdr_get_flags(const struct gtp2_hdr *gtp) +{ + return gtp->flags; +} + +static inline uint8_t gtp2_hdr_get_version(const struct gtp2_hdr *gtp) +{ + return (gtp->flags & GTP2_FLAG_VERSION) >> 5; +} + +static inline uint8_t gtp2_hdr_get_piggyback_flag(const struct gtp2_hdr *gtp) +{ + return (gtp->flags & GTP2_FLAG_PIGGYBACK) >> 4; +} + +static inline uint8_t gtp2_hdr_get_teid_flag(const struct gtp2_hdr *gtp) +{ + return (gtp->flags & GTP2_FLAG_TEID) >> 3; +} + +static inline uint8_t gtp2_hdr_get_spare_flag(const struct gtp2_hdr *gtp) +{ + return (gtp->flags & GPT2_FLAG_SPARE) >> 0; +} + +static inline uint8_t gtp2_hdr_get_msg_type(const struct gtp2_hdr *gtp) +{ + return gtp->msg_type; +} + +static inline uint16_t gtp2_hdr_get_msg_len(const struct gtp2_hdr *gtp) +{ + return ntohs(gtp->msg_len); +} + +static inline uint32_t gtp2_hdr_get_teid(const struct gtp2_hdr *gtp) +{ + if (gtp2_hdr_get_teid_flag(gtp)) + { + const struct gtp2_hdr_long *gtp_long = (const struct gtp2_hdr_long *)gtp; + return ntohl(gtp_long->teid); + } + else + { + return 0; + } +} + +static inline uint32_t gtp2_hdr_get_seq(const struct gtp2_hdr *gtp) +{ + if (gtp2_hdr_get_teid_flag(gtp)) + { + const struct gtp2_hdr_long *gtp_long = (const struct gtp2_hdr_long *)gtp; + return ntohl(gtp_long->seq_and_spare) >> 8; + } + else + { + return ntohl(gtp->seq_and_spare) >> 8; + } +} + +static inline uint8_t gtp2_hdr_get_spare(const struct gtp2_hdr *gtp) +{ + if (gtp2_hdr_get_teid_flag(gtp)) + { + const struct gtp2_hdr_long *gtp_long = (const struct gtp2_hdr_long *)gtp; + return ntohl(gtp_long->seq_and_spare) & 0xFF; + } + else + { + return ntohl(gtp->seq_and_spare) & 0xFF; + } +} + +static inline uint16_t calc_gtp2_hdr_len(const char *data, uint16_t len) +{ + if (len < sizeof(struct gtp2_hdr)) + { + return 0; + } + + const struct gtp2_hdr *gtp = (const struct gtp2_hdr *)data; + if (gtp2_hdr_get_version(gtp) == 2) + { + if (gtp2_hdr_get_teid_flag(gtp)) + { + return sizeof(struct gtp2_hdr_long); + } + else + { + return sizeof(struct gtp2_hdr); + } + } + + return 0; +} + +static inline void gtp2_hdr_set_flags(struct gtp2_hdr *gtp, uint8_t flags) +{ + gtp->flags = flags; +} + +static inline void gtp2_hdr_set_version(struct gtp2_hdr *gtp, uint8_t version) +{ + gtp->flags = (gtp->flags & ~GTP2_FLAG_VERSION) | (version << 5); +} + +static inline void gtp2_hdr_set_piggyback_flag(struct gtp2_hdr *gtp, uint8_t piggyback) +{ + gtp->flags = (gtp->flags & ~GTP2_FLAG_PIGGYBACK) | (piggyback << 4); +} + +static inline void gtp2_hdr_set_teid_flag(struct gtp2_hdr *gtp, uint8_t teid_flag) +{ + gtp->flags = (gtp->flags & ~GTP2_FLAG_TEID) | (teid_flag << 3); +} + +static inline void gtp2_hdr_set_spare_flag(struct gtp2_hdr *gtp, uint8_t spare_flag) +{ + gtp->flags = (gtp->flags & ~GPT2_FLAG_SPARE) | (spare_flag << 0); +} + +static inline void gtp2_hdr_set_msg_type(struct gtp2_hdr *gtp, uint8_t msg_type) +{ + gtp->msg_type = msg_type; +} + +static inline void gtp2_hdr_set_msg_len(struct gtp2_hdr *gtp, uint16_t msg_len) +{ + gtp->msg_len = htons(msg_len); +} + +static inline void gtp2_hdr_set_teid(struct gtp2_hdr *gtp, uint32_t teid) +{ + if (gtp2_hdr_get_teid_flag(gtp)) + { + struct gtp2_hdr_long *gtp_long = (struct gtp2_hdr_long *)gtp; + gtp_long->teid = htonl(teid); + } +} + +static inline void gtp2_hdr_set_seq(struct gtp2_hdr *gtp, uint32_t seq) +{ + if (gtp2_hdr_get_teid_flag(gtp)) + { + struct gtp2_hdr_long *gtp_long = (struct gtp2_hdr_long *)gtp; + gtp_long->seq_and_spare = htonl((seq << 8) | gtp2_hdr_get_spare(gtp)); + } + else + { + gtp->seq_and_spare = htonl((seq << 8) | gtp2_hdr_get_spare(gtp)); + } +} + +static inline void gtp2_hdr_set_spare(struct gtp2_hdr *gtp, uint8_t spare) +{ + if (gtp2_hdr_get_teid_flag(gtp)) + { + struct gtp2_hdr_long *gtp_long = (struct gtp2_hdr_long *)gtp; + gtp_long->seq_and_spare = htonl((gtp2_hdr_get_seq(gtp) << 8) | spare); + } + else + { + gtp->seq_and_spare = htonl((gtp2_hdr_get_seq(gtp) << 8) | spare); + } +} + +static inline int gtp2_hdr_to_str(const struct gtp2_hdr *gtp, char *buf, size_t len) +{ + memset(buf, 0, len); + + int used = 0; + used += snprintf(buf + used, len - used, "GTP: flags=0x%02x (version=%u, piggyback=%u, teid_flag=%u, spare_flag=%u), msg_type=0x%02x, msg_len=%u", + gtp2_hdr_get_flags(gtp), + gtp2_hdr_get_version(gtp), + gtp2_hdr_get_piggyback_flag(gtp), + gtp2_hdr_get_teid_flag(gtp), + gtp2_hdr_get_spare_flag(gtp), + gtp2_hdr_get_msg_type(gtp), + gtp2_hdr_get_msg_len(gtp)); + + if (gtp2_hdr_get_teid_flag(gtp)) + { + used += snprintf(buf + used, len - used, ", teid=%u", gtp2_hdr_get_teid(gtp)); + } + else + { + used += snprintf(buf + used, len - used, ", seq=%u, spare=%u", gtp2_hdr_get_seq(gtp), gtp2_hdr_get_spare(gtp)); + } + return used; +} + +/****************************************************************************** + * GTP + ******************************************************************************/ + +// return GTP version 0 or 1 +// return 255 if not GTP +static inline uint8_t peek_gtp_version(const char *data, uint16_t len) +{ + if (data == NULL || len == 0) + { + return 255; + } + + return ((*(uint8_t *)data) >> 5) & 0x07; +} + +static inline uint16_t calc_gtp_hdr_len(const char *data, uint32_t len) +{ + switch (peek_gtp_version(data, len)) + { + case 1: + return calc_gtp1_hdr_len(data, len); + case 2: + return calc_gtp2_hdr_len(data, len); + default: + return 0; + } +} + +static inline int gtp_hdr_to_str(const char *hdr, uint16_t len, char *buf, size_t size) +{ + switch (peek_gtp_version(hdr, len)) + { + case 1: + return gtp1_hdr_to_str((const struct gtp1_hdr *)hdr, buf, size); + case 2: + return gtp2_hdr_to_str((const struct gtp2_hdr *)hdr, buf, size); + default: + return 0; + } +} + +/****************************************************************************** + * IPv4 + ******************************************************************************/ + +/* + * Internet Header 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 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * |Version| IHL |Type of Service| Total Length | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Identification |Flags| Fragment Offset | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Time to Live | Protocol | Header Checksum | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Source Address | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Destination Address | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Options | Padding | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + */ + +static inline uint8_t ip4_hdr_get_version(const struct ip *hdr) +{ + return hdr->ip_v; +} + +// IP Options are included in the hdr_len field +static inline uint8_t ip4_hdr_get_hdr_len(const struct ip *hdr) +{ + return hdr->ip_hl << 2; +} + +static inline uint8_t ip4_hdr_get_tos(const struct ip *hdr) +{ + return hdr->ip_tos; +} + +static inline uint16_t ip4_hdr_get_total_len(const struct ip *hdr) +{ + return ntohs(hdr->ip_len); +} + +static inline uint16_t ip4_hdr_get_ipid(const struct ip *hdr) +{ + return ntohs(hdr->ip_id); +} + +static inline uint8_t ip4_hdr_get_flags(const struct ip *hdr) +{ + return (ntohs(hdr->ip_off) & (~IP_OFFMASK)) >> 13; +} + +static inline bool ip4_hdr_get_rf_flag(const struct ip *hdr) +{ + return (ntohs(hdr->ip_off) & IP_RF) != 0; +} + +static inline bool ip4_hdr_get_df_flag(const struct ip *hdr) +{ + return (ntohs(hdr->ip_off) & IP_DF) != 0; +} + +static inline bool ip4_hdr_get_mf_flag(const struct ip *hdr) +{ + return (ntohs(hdr->ip_off) & IP_MF) != 0; +} + +static inline uint16_t ip4_hdr_get_frag_offset(const struct ip *hdr) +{ + return (ntohs(hdr->ip_off) & IP_OFFMASK) << 3; +} + +static inline uint8_t ip4_hdr_get_ttl(const struct ip *hdr) +{ + return hdr->ip_ttl; +} + +static inline uint8_t ip4_hdr_get_proto(const struct ip *hdr) +{ + return hdr->ip_p; +} + +static inline uint16_t ip4_hdr_get_checksum(const struct ip *hdr) +{ + return ntohs(hdr->ip_sum); +} + +static inline uint32_t ip4_hdr_get_src_addr(const struct ip *hdr) +{ + return ntohl(hdr->ip_src.s_addr); +} + +static inline uint32_t ip4_hdr_get_dst_addr(const struct ip *hdr) +{ + return ntohl(hdr->ip_dst.s_addr); +} + +static inline struct in_addr ip4_hdr_get_src_in_addr(const struct ip *hdr) +{ + return hdr->ip_src; +} + +static inline struct in_addr ip4_hdr_get_dst_in_addr(const struct ip *hdr) +{ + return hdr->ip_dst; +} + +static inline uint8_t ip4_hdr_get_opt_len(const struct ip *hdr) +{ + return ip4_hdr_get_hdr_len(hdr) - sizeof(struct ip); +} + +static inline const char *ip4_hdr_get_opt_data(const struct ip *hdr) +{ + if (ip4_hdr_get_opt_len(hdr) == 0) + { + return NULL; + } + + return (const char *)hdr + sizeof(struct ip); +} + +static inline void ip4_hdr_set_version(struct ip *hdr, uint8_t version) +{ + hdr->ip_v = version; +} + +static inline void ip4_hdr_set_hdr_len(struct ip *hdr, uint8_t hdr_len) +{ + hdr->ip_hl = hdr_len >> 2; +} + +static inline void ip4_hdr_set_tos(struct ip *hdr, uint8_t tos) +{ + hdr->ip_tos = tos; +} + +static inline void ip4_hdr_set_total_len(struct ip *hdr, uint16_t total_len) +{ + hdr->ip_len = htons(total_len); +} + +static inline void ip4_hdr_set_ipid(struct ip *hdr, uint16_t ipid) +{ + hdr->ip_id = htons(ipid); +} + +static inline void ip4_hdr_set_flags(struct ip *hdr, uint8_t flags) +{ + hdr->ip_off = htons((flags << 13) | (ntohs(hdr->ip_off) & IP_OFFMASK)); +} + +static inline void ip4_hdr_set_rf_flag(struct ip *hdr, bool flag) +{ + if (flag) + { + hdr->ip_off = htons(ntohs(hdr->ip_off) | IP_RF); + } + else + { + hdr->ip_off = htons(ntohs(hdr->ip_off) & ~IP_RF); + } +} + +static inline void ip4_hdr_set_df_flag(struct ip *hdr, bool flag) +{ + if (flag) + { + hdr->ip_off = htons(ntohs(hdr->ip_off) | IP_DF); + } + else + { + hdr->ip_off = htons(ntohs(hdr->ip_off) & ~IP_DF); + } +} + +static inline void ip4_hdr_set_mf_flag(struct ip *hdr, bool flag) +{ + if (flag) + { + hdr->ip_off = htons(ntohs(hdr->ip_off) | IP_MF); + } + else + { + hdr->ip_off = htons(ntohs(hdr->ip_off) & ~IP_MF); + } +} + +static inline void ip4_hdr_set_frag_offset(struct ip *hdr, uint16_t frag_offset) +{ + hdr->ip_off = htons((frag_offset >> 3) | (ntohs(hdr->ip_off) & ~IP_OFFMASK)); +} + +static inline void ip4_hdr_set_ttl(struct ip *hdr, uint8_t ttl) +{ + hdr->ip_ttl = ttl; +} + +static inline void ip4_hdr_set_protocol(struct ip *hdr, uint8_t protocol) +{ + hdr->ip_p = protocol; +} + +static inline void ip4_hdr_set_checksum(struct ip *hdr, uint16_t checksum) +{ + hdr->ip_sum = htons(checksum); +} + +static inline void ip4_hdr_set_src_addr(struct ip *hdr, uint32_t saddr) +{ + hdr->ip_src.s_addr = htonl(saddr); +} + +static inline void ip4_hdr_set_dst_addr(struct ip *hdr, uint32_t daddr) +{ + hdr->ip_dst.s_addr = htonl(daddr); +} + +static inline void ip4_hdr_set_src_in_addr(struct ip *hdr, struct in_addr saddr) +{ + hdr->ip_src = saddr; +} + +static inline void ip4_hdr_set_dst_in_addr(struct ip *hdr, struct in_addr daddr) +{ + hdr->ip_dst = daddr; +} + +static inline void ip4_hdr_set_opt_len(struct ip *hdr, uint8_t opt_len) +{ + ip4_hdr_set_hdr_len(hdr, opt_len + sizeof(struct ip)); +} + +// must be called after ip4_hdr_set_opt_len +static inline void ip4_hdr_set_opt_data(struct ip *hdr, const char *opt_data) +{ + if (opt_data) + { + memcpy((char *)hdr + sizeof(struct ip), opt_data, ip4_hdr_get_opt_len(hdr)); + } +} + +// /usr/include/netinet/in.h +static inline int is_ip_proto(uint16_t proto) +{ + switch (proto) + { + case IPPROTO_IP: + case IPPROTO_ICMP: + case IPPROTO_IGMP: + case IPPROTO_IPIP: + case IPPROTO_TCP: + case IPPROTO_EGP: + case IPPROTO_PUP: + case IPPROTO_UDP: + case IPPROTO_IDP: + case IPPROTO_TP: + case IPPROTO_DCCP: + case IPPROTO_IPV6: + case IPPROTO_RSVP: + case IPPROTO_GRE: + case IPPROTO_ESP: + case IPPROTO_AH: + case IPPROTO_MTP: + case IPPROTO_BEETPH: + case IPPROTO_ENCAP: + case IPPROTO_PIM: + case IPPROTO_COMP: + case IPPROTO_SCTP: + case IPPROTO_UDPLITE: + case IPPROTO_MPLS: + case IPPROTO_ETHERNET: + case IPPROTO_RAW: + case IPPROTO_MPTCP: + return 1; + default: + return 0; + } +} + +static inline const char *ip_proto_to_str(uint16_t proto) +{ + switch (proto) + { + case IPPROTO_IP: + return "IPPROTO_IP"; + case IPPROTO_ICMP: + return "IPPROTO_ICMP"; + case IPPROTO_IGMP: + return "IPPROTO_IGMP"; + case IPPROTO_IPIP: + return "IPPROTO_IPIP"; + case IPPROTO_TCP: + return "IPPROTO_TCP"; + case IPPROTO_EGP: + return "IPPROTO_EGP"; + case IPPROTO_PUP: + return "IPPROTO_PUP"; + case IPPROTO_UDP: + return "IPPROTO_UDP"; + case IPPROTO_IDP: + return "IPPROTO_IDP"; + case IPPROTO_TP: + return "IPPROTO_TP"; + case IPPROTO_DCCP: + return "IPPROTO_DCCP"; + case IPPROTO_IPV6: + return "IPPROTO_IPV6"; + case IPPROTO_RSVP: + return "IPPROTO_RSVP"; + case IPPROTO_GRE: + return "IPPROTO_GRE"; + case IPPROTO_ESP: + return "IPPROTO_ESP"; + case IPPROTO_AH: + return "IPPROTO_AH"; + case IPPROTO_MTP: + return "IPPROTO_MTP"; + case IPPROTO_BEETPH: + return "IPPROTO_BEETPH"; + case IPPROTO_ENCAP: + return "IPPROTO_ENCAP"; + case IPPROTO_PIM: + return "IPPROTO_PIM"; + case IPPROTO_COMP: + return "IPPROTO_COMP"; + case IPPROTO_SCTP: + return "IPPROTO_SCTP"; + case IPPROTO_UDPLITE: + return "IPPROTO_UDPLITE"; + case IPPROTO_MPLS: + return "IPPROTO_MPLS"; + case IPPROTO_ETHERNET: + return "IPPROTO_ETHERNET"; + case IPPROTO_RAW: + return "IPPROTO_RAW"; + case IPPROTO_MPTCP: + return "IPPROTO_MPTCP"; + default: + return "IPPROTO_UNKNOWN"; + } +} + +static inline int ip4_hdr_to_str(const struct ip *hdr, char *buf, size_t size) +{ + memset(buf, 0, size); + char src_addr_str[INET6_ADDRSTRLEN] = {0}; + char dst_addr_str[INET6_ADDRSTRLEN] = {0}; + + uint16_t proto = ip4_hdr_get_proto(hdr); + struct in_addr src_addr = ip4_hdr_get_src_in_addr(hdr); + struct in_addr dst_addr = ip4_hdr_get_dst_in_addr(hdr); + inet_ntop(AF_INET, &src_addr, src_addr_str, sizeof(src_addr_str)); + inet_ntop(AF_INET, &dst_addr, dst_addr_str, sizeof(dst_addr_str)); + + return snprintf(buf, size, "IPv4: version=%u hdr_len=%u tos=%u total_len=%u ipid=%u flags=%u(rf=%u df=%u mf=%u) frag_offset=%u ttl=%u proto=%s checksum=0x%x src_addr=%s dst_addr=%s opt_len=%u", + ip4_hdr_get_version(hdr), ip4_hdr_get_hdr_len(hdr), ip4_hdr_get_tos(hdr), + ip4_hdr_get_total_len(hdr), ip4_hdr_get_ipid(hdr), ip4_hdr_get_flags(hdr), + ip4_hdr_get_rf_flag(hdr), ip4_hdr_get_df_flag(hdr), ip4_hdr_get_mf_flag(hdr), + ip4_hdr_get_frag_offset(hdr), ip4_hdr_get_ttl(hdr), ip_proto_to_str(proto), + ip4_hdr_get_checksum(hdr), src_addr_str, dst_addr_str, ip4_hdr_get_opt_len(hdr)); +} + +/****************************************************************************** + * IPv6 + ******************************************************************************/ + +/* + * IPv6 Header 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 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * |Version| Traffic Class | Flow Label | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Payload Length | Next Header | Hop Limit | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | | + * + + + * | | + * + Source Address + + * | | + * + + + * | | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | | + * + + + * | | + * + Destination Address + + * | | + * + + + * | | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * + * Fragment Header + * + * 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 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Next Header | Reserved | Fragment Offset |Res|M| + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Identification | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + */ + +static inline uint8_t ip6_hdr_get_version(const struct ip6_hdr *hdr) +{ + return (ntohl(hdr->ip6_flow) & 0xf0000000) >> 28; +} + +static inline uint8_t ip6_hdr_get_traffic_class(const struct ip6_hdr *hdr) +{ + return (ntohl(hdr->ip6_flow) & 0x0ff00000) >> 20; +} + +static inline uint32_t ip6_hdr_get_flow_label(const struct ip6_hdr *hdr) +{ + return ntohl(hdr->ip6_flow) & 0x000fffff; +} + +static inline uint16_t ip6_hdr_get_payload_len(const struct ip6_hdr *hdr) +{ + return ntohs(hdr->ip6_plen); +} + +static inline uint8_t ip6_hdr_get_next_header(const struct ip6_hdr *hdr) +{ + return hdr->ip6_nxt; +} + +static inline uint8_t ip6_hdr_get_hop_limit(const struct ip6_hdr *hdr) +{ + return hdr->ip6_hlim; +} + +static inline struct in6_addr ip6_hdr_get_src_in6_addr(const struct ip6_hdr *hdr) +{ + return hdr->ip6_src; +} + +static inline struct in6_addr ip6_hdr_get_dst_in6_addr(const struct ip6_hdr *hdr) +{ + return hdr->ip6_dst; +} + +static inline struct ip6_frag *ip6_hdr_get_frag_ext(const struct ip6_hdr *hdr) +{ + if (hdr->ip6_nxt != IPPROTO_FRAGMENT) + { + return NULL; + } + return (struct ip6_frag *)((char *)hdr + sizeof(struct ip6_hdr)); +} + +static inline void ip6_hdr_set_version(struct ip6_hdr *hdr, uint8_t version) +{ + hdr->ip6_flow = htonl((ntohl(hdr->ip6_flow) & 0x0fffffff) | (version << 28)); +} + +static inline void ip6_hdr_set_traffic_class(struct ip6_hdr *hdr, uint8_t traffic_class) +{ + hdr->ip6_flow = htonl((ntohl(hdr->ip6_flow) & 0xf00fffff) | (traffic_class << 20)); +} + +static inline void ip6_hdr_set_flow_label(struct ip6_hdr *hdr, uint32_t flow_label) +{ + hdr->ip6_flow = htonl((ntohl(hdr->ip6_flow) & 0xfff00000) | flow_label); +} + +static inline void ip6_hdr_set_payload_len(struct ip6_hdr *hdr, uint16_t payload_len) +{ + hdr->ip6_plen = htons(payload_len); +} + +static inline void ip6_hdr_set_next_header(struct ip6_hdr *hdr, uint8_t next_header) +{ + hdr->ip6_nxt = next_header; +} + +static inline void ip6_hdr_set_hop_limit(struct ip6_hdr *hdr, uint8_t hop_limit) +{ + hdr->ip6_hlim = hop_limit; +} + +static inline void ip6_hdr_set_src_in6_addr(struct ip6_hdr *hdr, struct in6_addr src_addr) +{ + hdr->ip6_src = src_addr; +} + +static inline void ip6_hdr_set_dst_in6_addr(struct ip6_hdr *hdr, struct in6_addr dst_addr) +{ + hdr->ip6_dst = dst_addr; +} + +static inline uint8_t ipv6_frag_get_next_header(const struct ip6_frag *frag) +{ + return frag->ip6f_nxt; +} + +static inline uint16_t ipv6_frag_get_offset(const struct ip6_frag *frag) +{ + return ntohs(frag->ip6f_offlg & IP6F_OFF_MASK); +} + +static inline uint32_t ipv6_frag_get_ident(const struct ip6_frag *frag) +{ + return ntohl(frag->ip6f_ident); +} + +static inline bool ipv6_frag_get_more(const struct ip6_frag *frag) +{ + return (frag->ip6f_offlg & IP6F_MORE_FRAG); +} + +static inline void ipv6_frag_set_next_header(struct ip6_frag *frag, uint8_t next_header) +{ + frag->ip6f_nxt = next_header; +} + +static inline void ipv6_frag_set_offset(struct ip6_frag *frag, uint16_t offset) +{ + frag->ip6f_offlg = (frag->ip6f_offlg & ~IP6F_OFF_MASK) | htons(offset); +} + +static inline void ipv6_frag_set_ident(struct ip6_frag *frag, uint32_t ident) +{ + frag->ip6f_ident = htonl(ident); +} + +static inline void ipv6_frag_set_more(struct ip6_frag *frag, bool more) +{ + if (more) + { + frag->ip6f_offlg |= IP6F_MORE_FRAG; + } + else + { + frag->ip6f_offlg &= ~IP6F_MORE_FRAG; + } +} + +static inline int ip6_hdr_to_str(const struct ip6_hdr *hdr, char *buf, size_t size) +{ + memset(buf, 0, size); + char src_addr_str[INET6_ADDRSTRLEN] = {0}; + char dst_addr_str[INET6_ADDRSTRLEN] = {0}; + + struct in6_addr src_addr = ip6_hdr_get_src_in6_addr(hdr); + struct in6_addr dst_addr = ip6_hdr_get_dst_in6_addr(hdr); + inet_ntop(AF_INET6, &src_addr, src_addr_str, INET6_ADDRSTRLEN); + inet_ntop(AF_INET6, &dst_addr, dst_addr_str, INET6_ADDRSTRLEN); + + return snprintf(buf, size, "IPv6: version=%u traffic_class=%u flow_label=%u payload_len=%u next_header=%u hop_limit=%u src_addr=%s dst_addr=%s", + ip6_hdr_get_version(hdr), ip6_hdr_get_traffic_class(hdr), ip6_hdr_get_flow_label(hdr), ip6_hdr_get_payload_len(hdr), + ip6_hdr_get_next_header(hdr), ip6_hdr_get_hop_limit(hdr), src_addr_str, dst_addr_str); +} + +/****************************************************************************** + * L2TP + ******************************************************************************/ + +struct l2tp_hdr +{ + uint16_t flags; + // other option fields +}; + +#define L2TP_CONTROL_BIT 0x8000 +#define L2TP_LENGTH_BIT 0x4000 +#define L2TP_SEQUENCE_BIT 0x0800 +#define L2TP_OFFSET_BIT 0x0200 +#define L2TP_PRIORITY_BIT 0x0100 +#define L2TP_VERSION 0x000f + +static inline uint8_t l2tp_hdr_get_ver(const struct l2tp_hdr *hdr) +{ + return ntohs(hdr->flags) & L2TP_VERSION; +} + +// 1: control message +// 0: data message +static inline uint8_t l2tp_hdr_get_type(const struct l2tp_hdr *hdr) +{ + return (ntohs(hdr->flags) & L2TP_CONTROL_BIT) >> 15; +} + +/* + * Layer Two Tunneling Protocol "L2TP" (V2) + * + * 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 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * |T|L|x|x|S|x|O|P|x|x|x|x| Ver | Length (opt) | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Tunnel ID | Session ID | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Ns (opt) | Nr (opt) | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Offset Size (opt) | Offset pad... (opt) + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * + * https://datatracker.ietf.org/doc/html/rfc2661 + */ + +static inline uint16_t calc_udp_l2tpv2_hdr_len(const char *data, uint16_t len) +{ + const struct l2tp_hdr *hdr = (const struct l2tp_hdr *)data; + uint16_t flags = ntohs(hdr->flags); + + // ctrl message + if (flags & L2TP_CONTROL_BIT) + { + if ((flags & L2TP_LENGTH_BIT) == 0 || (flags & L2TP_SEQUENCE_BIT) == 0 || + (flags & L2TP_OFFSET_BIT) != 0 || (flags & L2TP_PRIORITY_BIT) != 0) + { + return 0; + } + else + { + return ntohs(*((uint16_t *)(data + 2))); + } + } + // data message + else + { + uint16_t skip_len = 2; // skip flags field + if (flags & L2TP_LENGTH_BIT) + { + skip_len += 2; // skip length field + } + skip_len += 2; // skip tunnel id field + skip_len += 2; // skip session id field + if (flags & L2TP_SEQUENCE_BIT) + { + skip_len += 2; // skip ns field + skip_len += 2; // skip nr field + } + if (flags & L2TP_OFFSET_BIT) + { + if (skip_len + 2 > len) + { + return 0; + } + uint16_t offset = ntohs(*((uint16_t *)(data + skip_len))); + if (offset == 0) + { + skip_len += 2; // skip offset field + } + else + { + skip_len = offset; + } + } + return skip_len; + } +} + +/* + * Figure 4.1.1.1: L2TPv3 Session Header Over IP + * + * 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 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Session ID | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Cookie (optional, maximum 64 bits)... + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * + * Figure 4.1.1.2: L2TPv3 Control Message Header Over IP + * + * 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 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | (32 bits of zeros) | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * |T|L|x|x|S|x|x|x|x|x|x|x| Ver | Length | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Control Connection ID | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Ns | Nr | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * + * Note: Unlike L2TP over UDP, which uses the T bit to distinguish between + * L2TP control and data packets, L2TP over IP uses the reserved Session + * ID of zero (0) when sending control messages. + * + * https://www.rfc-editor.org/rfc/rfc3931.html + */ + +static inline uint16_t calc_ip_l2tpv3_hdr_len(const char *data, uint16_t len) +{ + if (len < 4) + { + return 0; + } + uint32_t session_id = ntohl(*((uint32_t *)data)); + // data message + if (session_id) + { + // TODO The optional Cookie field contains a variable-length value (maximum 64 bits) + // TODO L2-Specific Sublayer 4 bytes + return 4 + 4; + } + // control message + else + { + if (len < 16) + { + return 0; + } + uint16_t flags = ntohs(*((uint16_t *)(data + 4))); + if ((flags & L2TP_LENGTH_BIT) == 0 || (flags & L2TP_SEQUENCE_BIT) == 0) + { + return 0; + } + else + { + return ntohs(*((uint16_t *)(data + 4 + 2))); + } + } +} + +/* + * Layer Two Tunneling Protocol - Version 3 (L2TPv3) + * + * Figure 3.2.1: L2TP Control Message Header + * + * 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 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * |T|L|x|x|S|x|x|x|x|x|x|x| Ver | Length | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Control Connection ID | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Ns | Nr | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * + * Figure 4.1.2.1: L2TPv3 Session Header over UDP + * + * 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 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * |T|x|x|x|x|x|x|x|x|x|x|x| Ver | Reserved | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Session ID | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Cookie (optional, maximum 64 bits)... + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * + * https://www.rfc-editor.org/rfc/rfc3931.html + */ + +static inline uint16_t calc_udp_l2tpv3_hdr_len(const char *data, uint16_t len) +{ + if (len < 8) + { + return 0; + } + + const struct l2tp_hdr *hdr = (const struct l2tp_hdr *)data; + uint16_t flags = ntohs(hdr->flags); + + // ctrl message + if (flags & L2TP_CONTROL_BIT) + { + if ((flags & L2TP_LENGTH_BIT) == 0 || (flags & L2TP_SEQUENCE_BIT) == 0) + { + return 0; + } + else + { + return ntohs(*((uint16_t *)(data + 2))); + } + } + // data message + else + { + // TODO The optional Cookie field contains a variable-length value (maximum 64 bits) + // TODO L2-Specific Sublayer 4 bytes + return 8 + 4; + } +} + +static inline int l2tp_hdr_to_str(const struct l2tp_hdr *hdr, char *buf, size_t size) +{ + memset(buf, 0, size); + return snprintf(buf, size, "L2TP: type=%s version=%u", + l2tp_hdr_get_type(hdr) ? "control" : "data", l2tp_hdr_get_ver(hdr)); +} + +/****************************************************************************** + * MPLS + ******************************************************************************/ + +/* + * 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 | TC |S| TTL | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + */ + +static inline uint32_t mpls_label_get_label(const struct mpls_label *hdr) +{ + return ((ntohl(hdr->entry) & MPLS_LS_LABEL_MASK) >> MPLS_LS_LABEL_SHIFT); +} + +static inline uint8_t mpls_label_get_tc(const struct mpls_label *hdr) +{ + return ((ntohl(hdr->entry) & MPLS_LS_TC_MASK) >> MPLS_LS_TC_SHIFT); +} + +static inline uint8_t mpls_label_get_bos(const struct mpls_label *hdr) +{ + return ((ntohl(hdr->entry) & MPLS_LS_S_MASK) >> MPLS_LS_S_SHIFT); +} + +static inline uint8_t mpls_label_get_ttl(const struct mpls_label *hdr) +{ + return ((ntohl(hdr->entry) & MPLS_LS_TTL_MASK) >> MPLS_LS_TTL_SHIFT); +} + +static inline void mpls_label_set_label(struct mpls_label *hdr, uint32_t label) +{ + hdr->entry = htonl((ntohl(hdr->entry) & ~MPLS_LS_LABEL_MASK) | (label << MPLS_LS_LABEL_SHIFT)); +} + +static inline void mpls_label_set_tc(struct mpls_label *hdr, uint8_t tc) +{ + hdr->entry = htonl((ntohl(hdr->entry) & ~MPLS_LS_TC_MASK) | (tc << MPLS_LS_TC_SHIFT)); +} + +static inline void mpls_label_set_bos(struct mpls_label *hdr, uint8_t bos) +{ + hdr->entry = htonl((ntohl(hdr->entry) & ~MPLS_LS_S_MASK) | (bos << MPLS_LS_S_SHIFT)); +} + +static inline void mpls_label_set_ttl(struct mpls_label *hdr, uint8_t ttl) +{ + hdr->entry = htonl((ntohl(hdr->entry) & ~MPLS_LS_TTL_MASK) | (ttl << MPLS_LS_TTL_SHIFT)); +} + +static inline int mpls_label_to_str(const struct mpls_label *hdr, char *buf, size_t size) +{ + return snprintf(buf, size, "MPLS: label=%u tc=%u bos=%u ttl=%u", + mpls_label_get_label(hdr), mpls_label_get_tc(hdr), + mpls_label_get_bos(hdr), mpls_label_get_ttl(hdr)); +} + +/****************************************************************************** + * TCP + ******************************************************************************/ + +/* + * TCP Header 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 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Source Port | Destination Port | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Sequence Number | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Acknowledgment Number | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Data | |U|A|P|R|S|F| | + * | Offset| Reserved |R|C|S|S|Y|I| Window | + * | | |G|K|H|T|N|N| | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Checksum | Urgent Pointer | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Options | Padding | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | data | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + */ + +static inline uint16_t tcp_hdr_get_src_port(const struct tcphdr *hdr) +{ + return ntohs(hdr->th_sport); +} + +static inline uint16_t tcp_hdr_get_dst_port(const struct tcphdr *hdr) +{ + return ntohs(hdr->th_dport); +} + +static inline uint32_t tcp_hdr_get_seq(const struct tcphdr *hdr) +{ + return ntohl(hdr->th_seq); +} + +static inline uint32_t tcp_hdr_get_ack(const struct tcphdr *hdr) +{ + return ntohl(hdr->th_ack); +} + +static inline uint8_t tcp_hdr_get_hdr_len(const struct tcphdr *hdr) +{ + return hdr->th_off << 2; +} + +static inline uint8_t tcp_hdr_get_flags(const struct tcphdr *hdr) +{ + return hdr->th_flags; +} + +static inline bool tcp_hdr_get_urg_flag(const struct tcphdr *hdr) +{ + return hdr->th_flags & TH_URG; +} + +static inline bool tcp_hdr_get_ack_flag(const struct tcphdr *hdr) +{ + return hdr->th_flags & TH_ACK; +} + +static inline bool tcp_hdr_get_push_flag(const struct tcphdr *hdr) +{ + return hdr->th_flags & TH_PUSH; +} + +static inline bool tcp_hdr_get_rst_flag(const struct tcphdr *hdr) +{ + return hdr->th_flags & TH_RST; +} + +static inline bool tcp_hdr_get_syn_flag(const struct tcphdr *hdr) +{ + return hdr->th_flags & TH_SYN; +} + +static inline bool tcp_hdr_get_fin_flag(const struct tcphdr *hdr) +{ + return hdr->th_flags & TH_FIN; +} + +static inline uint16_t tcp_hdr_get_window(const struct tcphdr *hdr) +{ + return ntohs(hdr->th_win); +} + +static inline uint16_t tcp_hdr_get_checksum(const struct tcphdr *hdr) +{ + return ntohs(hdr->th_sum); +} + +static inline uint16_t tcp_hdr_get_urg_ptr(const struct tcphdr *hdr) +{ + return ntohs(hdr->th_urp); +} + +static inline uint16_t tcp_hdr_get_opt_len(const struct tcphdr *hdr) +{ + return tcp_hdr_get_hdr_len(hdr) - sizeof(struct tcphdr); +} + +static inline const char *tcp_hdr_get_opt_data(const struct tcphdr *hdr) +{ + if (tcp_hdr_get_opt_len(hdr) == 0) + { + return NULL; + } + + return ((const char *)hdr) + sizeof(struct tcphdr); +} + +static inline void tcp_hdr_set_src_port(struct tcphdr *hdr, uint16_t port) +{ + hdr->th_sport = htons(port); +} + +static inline void tcp_hdr_set_dst_port(struct tcphdr *hdr, uint16_t port) +{ + hdr->th_dport = htons(port); +} + +static inline void tcp_hdr_set_seq(struct tcphdr *hdr, uint32_t seq) +{ + hdr->th_seq = htonl(seq); +} + +static inline void tcp_hdr_set_ack(struct tcphdr *hdr, uint32_t ack) +{ + hdr->th_ack = htonl(ack); +} + +static inline void tcp_hdr_set_hdr_len(struct tcphdr *hdr, uint8_t offset) +{ + hdr->th_off = offset >> 2; +} + +static inline void tcp_hdr_set_flags(struct tcphdr *hdr, uint8_t flags) +{ + hdr->th_flags = flags; +} + +static inline void tcp_hdr_set_urg_flag(struct tcphdr *hdr, bool flag) +{ + if (flag) + { + hdr->th_flags |= TH_URG; + } + else + { + hdr->th_flags &= ~TH_URG; + } +} + +static inline void tcp_hdr_set_ack_flag(struct tcphdr *hdr, bool flag) +{ + if (flag) + { + hdr->th_flags |= TH_ACK; + } + else + { + hdr->th_flags &= ~TH_ACK; + } +} + +static inline void tcp_hdr_set_push_flag(struct tcphdr *hdr, bool flag) +{ + if (flag) + { + hdr->th_flags |= TH_PUSH; + } + else + { + hdr->th_flags &= ~TH_PUSH; + } +} + +static inline void tcp_hdr_set_rst_flag(struct tcphdr *hdr, bool flag) +{ + if (flag) + { + hdr->th_flags |= TH_RST; + } + else + { + hdr->th_flags &= ~TH_RST; + } +} + +static inline void tcp_hdr_set_syn_flag(struct tcphdr *hdr, bool flag) +{ + if (flag) + { + hdr->th_flags |= TH_SYN; + } + else + { + hdr->th_flags &= ~TH_SYN; + } +} + +static inline void tcp_hdr_set_fin_flag(struct tcphdr *hdr, bool flag) +{ + if (flag) + { + hdr->th_flags |= TH_FIN; + } + else + { + hdr->th_flags &= ~TH_FIN; + } +} + +static inline void tcp_hdr_set_window(struct tcphdr *hdr, uint16_t window) +{ + hdr->th_win = htons(window); +} + +static inline void tcp_hdr_set_checksum(struct tcphdr *hdr, uint16_t checksum) +{ + hdr->th_sum = htons(checksum); +} + +static inline void tcp_hdr_set_urg_ptr(struct tcphdr *hdr, uint16_t ptr) +{ + hdr->th_urp = htons(ptr); +} + +static inline void tcp_hdr_set_opt_len(struct tcphdr *hdr, uint16_t len) +{ + hdr->th_off = (sizeof(struct tcphdr) + len) >> 2; +} + +// must be called after tcp_hdr_set_opt_len +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)); +} + +static inline int tcp_hdr_to_str(const struct tcphdr *hdr, char *buf, size_t size) +{ + memset(buf, 0, size); + return snprintf(buf, size, "TCP: src_port=%u dst_port=%u seq=%u ack=%u hdr_len=%u flags=0x%02x(%s%s%s%s%s%s) window=%u checksum=0x%x urg_ptr=%u opt_len=%u", + tcp_hdr_get_src_port(hdr), tcp_hdr_get_dst_port(hdr), + tcp_hdr_get_seq(hdr), tcp_hdr_get_ack(hdr), + tcp_hdr_get_hdr_len(hdr), tcp_hdr_get_flags(hdr), + tcp_hdr_get_urg_flag(hdr) ? "URG " : "", + tcp_hdr_get_ack_flag(hdr) ? "ACK " : "", + tcp_hdr_get_push_flag(hdr) ? "PSH " : "", + tcp_hdr_get_rst_flag(hdr) ? "RST " : "", + tcp_hdr_get_syn_flag(hdr) ? "SYN " : "", + tcp_hdr_get_fin_flag(hdr) ? "FIN " : "", + tcp_hdr_get_window(hdr), tcp_hdr_get_checksum(hdr), + tcp_hdr_get_urg_ptr(hdr), tcp_hdr_get_opt_len(hdr)); +} + +/****************************************************************************** + * UDP + ******************************************************************************/ + +/* + * User Datagram Header 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 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Source Port | Destination Port | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Length | Checksum | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Data | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + */ + +static inline uint16_t udp_hdr_get_src_port(const struct udphdr *hdr) +{ + return ntohs(hdr->uh_sport); +} + +static inline uint16_t udp_hdr_get_dst_port(const struct udphdr *hdr) +{ + return ntohs(hdr->uh_dport); +} + +static inline uint16_t udp_hdr_get_total_len(const struct udphdr *hdr) +{ + return ntohs(hdr->uh_ulen); +} + +static inline uint16_t udp_hdr_get_checksum(const struct udphdr *hdr) +{ + return ntohs(hdr->uh_sum); +} + +static inline void udp_hdr_set_src_port(struct udphdr *hdr, uint16_t port) +{ + hdr->uh_sport = htons(port); +} + +static inline void udp_hdr_set_dst_port(struct udphdr *hdr, uint16_t port) +{ + hdr->uh_dport = htons(port); +} + +static inline void udp_hdr_set_total_len(struct udphdr *hdr, uint16_t len) +{ + hdr->uh_ulen = htons(len); +} + +static inline void udp_hdr_set_checksum(struct udphdr *hdr, uint16_t sum) +{ + hdr->uh_sum = htons(sum); +} + +static inline int udp_hdr_to_str(const struct udphdr *hdr, char *buf, size_t size) +{ + memset(buf, 0, size); + return snprintf(buf, size, "UDP: src_port=%u dst_port=%u total_len=%u checksum=0x%x", + udp_hdr_get_src_port(hdr), udp_hdr_get_dst_port(hdr), + udp_hdr_get_total_len(hdr), udp_hdr_get_checksum(hdr)); +} + +/****************************************************************************** + * VLAN + ******************************************************************************/ + +/* + * VLAN Header 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 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Pri |I| VLAN ID | Ethertype | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + */ + +struct vlan_hdr +{ + uint16_t vlan_tci; + uint16_t vlan_ethertype; +}; + +static inline uint8_t vlan_hdr_get_priority(const struct vlan_hdr *hdr) +{ + return (ntohs(hdr->vlan_tci) & 0xe000) >> 13; +} + +static inline uint8_t vlan_hdr_get_dei(const struct vlan_hdr *hdr) +{ + return (ntohs(hdr->vlan_tci) & 0x1000) >> 12; +} + +static inline uint16_t vlan_hdr_get_vid(const struct vlan_hdr *hdr) +{ + return ntohs(hdr->vlan_tci) & 0x0fff; +} + +static inline uint16_t vlan_hdr_get_ethertype(const struct vlan_hdr *hdr) +{ + return ntohs(hdr->vlan_ethertype); +} + +static inline void vlan_hdr_set_priority(struct vlan_hdr *hdr, uint8_t priority) +{ + hdr->vlan_tci = htons((ntohs(hdr->vlan_tci) & ~0xe000) | (priority << 13)); +} + +static inline void vlan_hdr_set_dei(struct vlan_hdr *hdr, uint8_t dei) +{ + hdr->vlan_tci = htons((ntohs(hdr->vlan_tci) & ~0x1000) | (dei << 12)); +} + +static inline void vlan_hdr_set_vid(struct vlan_hdr *hdr, uint16_t vid) +{ + hdr->vlan_tci = htons((ntohs(hdr->vlan_tci) & ~0x0fff) | vid); +} + +static inline void vlan_hdr_set_ethertype(struct vlan_hdr *hdr, uint16_t ethertype) +{ + hdr->vlan_ethertype = htons(ethertype); +} + +static inline int vlan_hdr_to_str(const struct vlan_hdr *hdr, char *buf, size_t size) +{ + memset(buf, 0, size); + uint16_t proto = vlan_hdr_get_ethertype(hdr); + return snprintf(buf, size, "VLAN: priority=%u dei=%u id=%u ethertype=%s", + vlan_hdr_get_priority(hdr), vlan_hdr_get_dei(hdr), + vlan_hdr_get_vid(hdr), eth_proto_to_str(proto)); +} + +/****************************************************************************** + * VXLAN + ******************************************************************************/ + +/* + * VXLAN Header + * + * 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 + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * |R|R|R|R|I|R|R|R| Reserved | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | VXLAN Network Identifier (VNI) | Reserved | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + */ + +struct vxlan_hdr +{ + uint32_t vxlan_flags; + uint32_t vxlan_vni; +}; + +static inline uint8_t vxlan_hdr_get_flags(const struct vxlan_hdr *hdr) +{ + return (ntohl(hdr->vxlan_flags) & 0xff000000) >> 24; +} + +static inline uint32_t vxlan_hdr_get_vni(const struct vxlan_hdr *hdr) +{ + return (ntohl(hdr->vxlan_vni) & 0xffffff00) >> 8; +} + +static inline void vxlan_hdr_set_flags(struct vxlan_hdr *hdr, uint8_t flags) +{ + hdr->vxlan_flags = htonl((ntohl(hdr->vxlan_flags) & ~0xff000000) | (flags << 24)); +} + +static inline void vxlan_hdr_set_vni(struct vxlan_hdr *hdr, uint32_t vni) +{ + hdr->vxlan_vni = htonl((ntohl(hdr->vxlan_vni) & ~0xffffff00) | (vni << 8)); +} + +static inline int vxlan_hdr_to_str(const struct vxlan_hdr *hdr, char *buf, size_t size) +{ + memset(buf, 0, size); + return snprintf(buf, size, "VXLAN: flags=%u vni=%u", + vxlan_hdr_get_flags(hdr), vxlan_hdr_get_vni(hdr)); +} + +#ifdef __cplusplus +} +#endif diff --git a/src/packet/packet_parser.cpp b/src/packet/packet_parser.cpp index 9d689af..ada09f3 100644 --- a/src/packet/packet_parser.cpp +++ b/src/packet/packet_parser.cpp @@ -5,17 +5,7 @@ #include #include "log.h" -#include "eth_utils.h" -#include "gre_utils.h" -#include "udp_utils.h" -#include "tcp_utils.h" -#include "ip4_utils.h" -#include "ip6_utils.h" -#include "gtp_utils.h" -#include "mpls_utils.h" -#include "l2tp_utils.h" -#include "vlan_utils.h" -#include "vxlan_utils.h" +#include "packet_helper.h" #include "packet_private.h" #include "packet_parser.h" diff --git a/src/packet/packet_utils.cpp b/src/packet/packet_utils.cpp index 759bf17..d9f8107 100644 --- a/src/packet/packet_utils.cpp +++ b/src/packet/packet_utils.cpp @@ -2,10 +2,7 @@ #include "log.h" #include "tuple.h" -#include "tcp_utils.h" -#include "udp_utils.h" -#include "ip4_utils.h" -#include "ip6_utils.h" +#include "packet_helper.h" #include "packet_private.h" #include "packet_utils.h" diff --git a/src/packet/tcp_utils.h b/src/packet/tcp_utils.h deleted file mode 100644 index f01fe62..0000000 --- a/src/packet/tcp_utils.h +++ /dev/null @@ -1,287 +0,0 @@ -#pragma once - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include -#include -#include -#define __FAVOR_BSD 1 -#include - -/* - * TCP Header 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 - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Source Port | Destination Port | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Sequence Number | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Acknowledgment Number | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Data | |U|A|P|R|S|F| | - * | Offset| Reserved |R|C|S|S|Y|I| Window | - * | | |G|K|H|T|N|N| | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Checksum | Urgent Pointer | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Options | Padding | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | data | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - */ - -/****************************************************************************** - * get - ******************************************************************************/ - -static inline uint16_t tcp_hdr_get_src_port(const struct tcphdr *hdr) -{ - return ntohs(hdr->th_sport); -} - -static inline uint16_t tcp_hdr_get_dst_port(const struct tcphdr *hdr) -{ - return ntohs(hdr->th_dport); -} - -static inline uint32_t tcp_hdr_get_seq(const struct tcphdr *hdr) -{ - return ntohl(hdr->th_seq); -} - -static inline uint32_t tcp_hdr_get_ack(const struct tcphdr *hdr) -{ - return ntohl(hdr->th_ack); -} - -static inline uint8_t tcp_hdr_get_hdr_len(const struct tcphdr *hdr) -{ - return hdr->th_off << 2; -} - -static inline uint8_t tcp_hdr_get_flags(const struct tcphdr *hdr) -{ - return hdr->th_flags; -} - -static inline bool tcp_hdr_get_urg_flag(const struct tcphdr *hdr) -{ - return hdr->th_flags & TH_URG; -} - -static inline bool tcp_hdr_get_ack_flag(const struct tcphdr *hdr) -{ - return hdr->th_flags & TH_ACK; -} - -static inline bool tcp_hdr_get_push_flag(const struct tcphdr *hdr) -{ - return hdr->th_flags & TH_PUSH; -} - -static inline bool tcp_hdr_get_rst_flag(const struct tcphdr *hdr) -{ - return hdr->th_flags & TH_RST; -} - -static inline bool tcp_hdr_get_syn_flag(const struct tcphdr *hdr) -{ - return hdr->th_flags & TH_SYN; -} - -static inline bool tcp_hdr_get_fin_flag(const struct tcphdr *hdr) -{ - return hdr->th_flags & TH_FIN; -} - -static inline uint16_t tcp_hdr_get_window(const struct tcphdr *hdr) -{ - return ntohs(hdr->th_win); -} - -static inline uint16_t tcp_hdr_get_checksum(const struct tcphdr *hdr) -{ - return ntohs(hdr->th_sum); -} - -static inline uint16_t tcp_hdr_get_urg_ptr(const struct tcphdr *hdr) -{ - return ntohs(hdr->th_urp); -} - -static inline uint16_t tcp_hdr_get_opt_len(const struct tcphdr *hdr) -{ - return tcp_hdr_get_hdr_len(hdr) - sizeof(struct tcphdr); -} - -static inline const char *tcp_hdr_get_opt_data(const struct tcphdr *hdr) -{ - if (tcp_hdr_get_opt_len(hdr) == 0) - { - return NULL; - } - - return ((const char *)hdr) + sizeof(struct tcphdr); -} - -/****************************************************************************** - * set - ******************************************************************************/ - -static inline void tcp_hdr_set_src_port(struct tcphdr *hdr, uint16_t port) -{ - hdr->th_sport = htons(port); -} - -static inline void tcp_hdr_set_dst_port(struct tcphdr *hdr, uint16_t port) -{ - hdr->th_dport = htons(port); -} - -static inline void tcp_hdr_set_seq(struct tcphdr *hdr, uint32_t seq) -{ - hdr->th_seq = htonl(seq); -} - -static inline void tcp_hdr_set_ack(struct tcphdr *hdr, uint32_t ack) -{ - hdr->th_ack = htonl(ack); -} - -static inline void tcp_hdr_set_hdr_len(struct tcphdr *hdr, uint8_t offset) -{ - hdr->th_off = offset >> 2; -} - -static inline void tcp_hdr_set_flags(struct tcphdr *hdr, uint8_t flags) -{ - hdr->th_flags = flags; -} - -static inline void tcp_hdr_set_urg_flag(struct tcphdr *hdr, bool flag) -{ - if (flag) - { - hdr->th_flags |= TH_URG; - } - else - { - hdr->th_flags &= ~TH_URG; - } -} - -static inline void tcp_hdr_set_ack_flag(struct tcphdr *hdr, bool flag) -{ - if (flag) - { - hdr->th_flags |= TH_ACK; - } - else - { - hdr->th_flags &= ~TH_ACK; - } -} - -static inline void tcp_hdr_set_push_flag(struct tcphdr *hdr, bool flag) -{ - if (flag) - { - hdr->th_flags |= TH_PUSH; - } - else - { - hdr->th_flags &= ~TH_PUSH; - } -} - -static inline void tcp_hdr_set_rst_flag(struct tcphdr *hdr, bool flag) -{ - if (flag) - { - hdr->th_flags |= TH_RST; - } - else - { - hdr->th_flags &= ~TH_RST; - } -} - -static inline void tcp_hdr_set_syn_flag(struct tcphdr *hdr, bool flag) -{ - if (flag) - { - hdr->th_flags |= TH_SYN; - } - else - { - hdr->th_flags &= ~TH_SYN; - } -} - -static inline void tcp_hdr_set_fin_flag(struct tcphdr *hdr, bool flag) -{ - if (flag) - { - hdr->th_flags |= TH_FIN; - } - else - { - hdr->th_flags &= ~TH_FIN; - } -} - -static inline void tcp_hdr_set_window(struct tcphdr *hdr, uint16_t window) -{ - hdr->th_win = htons(window); -} - -static inline void tcp_hdr_set_checksum(struct tcphdr *hdr, uint16_t checksum) -{ - hdr->th_sum = htons(checksum); -} - -static inline void tcp_hdr_set_urg_ptr(struct tcphdr *hdr, uint16_t ptr) -{ - hdr->th_urp = htons(ptr); -} - -static inline void tcp_hdr_set_opt_len(struct tcphdr *hdr, uint16_t len) -{ - hdr->th_off = (sizeof(struct tcphdr) + len) >> 2; -} - -// must be called after tcp_hdr_set_opt_len -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); - return snprintf(buf, size, "TCP: src_port=%u dst_port=%u seq=%u ack=%u hdr_len=%u flags=0x%02x(%s%s%s%s%s%s) window=%u checksum=0x%x urg_ptr=%u opt_len=%u", - tcp_hdr_get_src_port(hdr), tcp_hdr_get_dst_port(hdr), - tcp_hdr_get_seq(hdr), tcp_hdr_get_ack(hdr), - tcp_hdr_get_hdr_len(hdr), tcp_hdr_get_flags(hdr), - tcp_hdr_get_urg_flag(hdr) ? "URG " : "", - tcp_hdr_get_ack_flag(hdr) ? "ACK " : "", - tcp_hdr_get_push_flag(hdr) ? "PSH " : "", - tcp_hdr_get_rst_flag(hdr) ? "RST " : "", - tcp_hdr_get_syn_flag(hdr) ? "SYN " : "", - tcp_hdr_get_fin_flag(hdr) ? "FIN " : "", - tcp_hdr_get_window(hdr), tcp_hdr_get_checksum(hdr), - tcp_hdr_get_urg_ptr(hdr), tcp_hdr_get_opt_len(hdr)); -} - -#ifdef __cplusplus -} -#endif diff --git a/src/packet/test/gtest_eth_utils.cpp b/src/packet/test/gtest_eth_utils.cpp index 07704f2..e00f032 100644 --- a/src/packet/test/gtest_eth_utils.cpp +++ b/src/packet/test/gtest_eth_utils.cpp @@ -1,6 +1,6 @@ #include -#include "eth_utils.h" +#include "packet_helper.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) diff --git a/src/packet/test/gtest_gre0_utils.cpp b/src/packet/test/gtest_gre0_utils.cpp index 03852c0..e195f42 100644 --- a/src/packet/test/gtest_gre0_utils.cpp +++ b/src/packet/test/gtest_gre0_utils.cpp @@ -1,6 +1,6 @@ #include -#include "gre0_utils.h" +#include "packet_helper.h" /* * Generic Routing Encapsulation (IP) diff --git a/src/packet/test/gtest_gre1_utils.cpp b/src/packet/test/gtest_gre1_utils.cpp index cb91e12..92669f7 100644 --- a/src/packet/test/gtest_gre1_utils.cpp +++ b/src/packet/test/gtest_gre1_utils.cpp @@ -1,6 +1,6 @@ #include -#include "gre1_utils.h" +#include "packet_helper.h" /* * Generic Routing Encapsulation (PPP) diff --git a/src/packet/test/gtest_gtp1_utils.cpp b/src/packet/test/gtest_gtp1_utils.cpp index 45b220a..270d36a 100644 --- a/src/packet/test/gtest_gtp1_utils.cpp +++ b/src/packet/test/gtest_gtp1_utils.cpp @@ -1,6 +1,6 @@ #include -#include "gtp1_utils.h" +#include "packet_helper.h" /* * User Datagram Protocol, Src Port: 2123, Dst Port: 2123 diff --git a/src/packet/test/gtest_gtp2_utils.cpp b/src/packet/test/gtest_gtp2_utils.cpp index e02e4a7..584abf7 100644 --- a/src/packet/test/gtest_gtp2_utils.cpp +++ b/src/packet/test/gtest_gtp2_utils.cpp @@ -1,6 +1,6 @@ #include -#include "gtp2_utils.h" +#include "packet_helper.h" /* * User Datagram Protocol, Src Port: 2123, Dst Port: 12491 diff --git a/src/packet/test/gtest_ip4_utils.cpp b/src/packet/test/gtest_ip4_utils.cpp index b830e3f..f6e98a1 100644 --- a/src/packet/test/gtest_ip4_utils.cpp +++ b/src/packet/test/gtest_ip4_utils.cpp @@ -1,6 +1,6 @@ #include -#include "ip4_utils.h" +#include "packet_helper.h" /****************************************************************************** * more fragment diff --git a/src/packet/test/gtest_ip6_utils.cpp b/src/packet/test/gtest_ip6_utils.cpp index 533ed2c..94a51f8 100644 --- a/src/packet/test/gtest_ip6_utils.cpp +++ b/src/packet/test/gtest_ip6_utils.cpp @@ -1,6 +1,6 @@ #include -#include "ip6_utils.h" +#include "packet_helper.h" /* * Internet Protocol Version 6, Src: fe80::250:56ff:fe69:dc00, Dst: ff02::1:2 diff --git a/src/packet/test/gtest_l2tp_utils.cpp b/src/packet/test/gtest_l2tp_utils.cpp index 321b61f..3eb16bd 100644 --- a/src/packet/test/gtest_l2tp_utils.cpp +++ b/src/packet/test/gtest_l2tp_utils.cpp @@ -1,6 +1,6 @@ #include -#include "l2tp_utils.h" +#include "packet_helper.h" /* * Layer 2 Tunneling Protocol diff --git a/src/packet/test/gtest_mpls_utils.cpp b/src/packet/test/gtest_mpls_utils.cpp index 1ec38ce..9d3f0d8 100644 --- a/src/packet/test/gtest_mpls_utils.cpp +++ b/src/packet/test/gtest_mpls_utils.cpp @@ -1,6 +1,6 @@ #include -#include "mpls_utils.h" +#include "packet_helper.h" /* * MultiProtocol Label Switching Header, Label: 779408, Exp: 0, S: 0, TTL: 255 diff --git a/src/packet/test/gtest_packet_builder.cpp b/src/packet/test/gtest_packet_builder.cpp index 350308f..8e4f021 100644 --- a/src/packet/test/gtest_packet_builder.cpp +++ b/src/packet/test/gtest_packet_builder.cpp @@ -2,12 +2,7 @@ #include #include "checksum.h" -#include "tcp_utils.h" -#include "udp_utils.h" -#include "ip4_utils.h" -#include "ip6_utils.h" -#include "gtp_utils.h" -#include "gre_utils.h" +#include "packet_helper.h" #include "packet_private.h" #include "packet_dump.h" #include "packet_layer.h" diff --git a/src/packet/test/gtest_tcp_utils.cpp b/src/packet/test/gtest_tcp_utils.cpp index fd13102..47223e4 100644 --- a/src/packet/test/gtest_tcp_utils.cpp +++ b/src/packet/test/gtest_tcp_utils.cpp @@ -1,6 +1,6 @@ #include -#include "tcp_utils.h" +#include "packet_helper.h" /* * Transmission Control Protocol, Src Port: 55555, Dst Port: 40856, Seq: 0, Ack: 1, Len: 0 diff --git a/src/packet/test/gtest_tunnel.cpp b/src/packet/test/gtest_tunnel.cpp index e728181..7f1e266 100644 --- a/src/packet/test/gtest_tunnel.cpp +++ b/src/packet/test/gtest_tunnel.cpp @@ -1,10 +1,6 @@ #include -#include "udp_utils.h" -#include "gre_utils.h" -#include "ip4_utils.h" -#include "ip6_utils.h" -#include "vxlan_utils.h" +#include "packet_helper.h" #include "packet_private.h" #include "packet_parser.h" #include "packet_dump.h" diff --git a/src/packet/test/gtest_udp_utils.cpp b/src/packet/test/gtest_udp_utils.cpp index 62dbc81..d8b2c9a 100644 --- a/src/packet/test/gtest_udp_utils.cpp +++ b/src/packet/test/gtest_udp_utils.cpp @@ -1,6 +1,6 @@ #include -#include "udp_utils.h" +#include "packet_helper.h" /* * User Datagram Protocol, Src Port: 4001, Dst Port: 8000 diff --git a/src/packet/test/gtest_vlan_utils.cpp b/src/packet/test/gtest_vlan_utils.cpp index 1506f69..0832ef1 100644 --- a/src/packet/test/gtest_vlan_utils.cpp +++ b/src/packet/test/gtest_vlan_utils.cpp @@ -1,6 +1,6 @@ #include -#include "vlan_utils.h" +#include "packet_helper.h" /* * IEEE 802.1ad, ID: 1 diff --git a/src/packet/test/gtest_vxlan_utils.cpp b/src/packet/test/gtest_vxlan_utils.cpp index 5e22f2a..81a24fe 100644 --- a/src/packet/test/gtest_vxlan_utils.cpp +++ b/src/packet/test/gtest_vxlan_utils.cpp @@ -1,6 +1,6 @@ #include -#include "vxlan_utils.h" +#include "packet_helper.h" /* * Virtual eXtensible Local Area Network diff --git a/src/packet/udp_utils.h b/src/packet/udp_utils.h deleted file mode 100644 index 0c2a767..0000000 --- a/src/packet/udp_utils.h +++ /dev/null @@ -1,90 +0,0 @@ -#pragma once - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include -#include -#include -#define __FAVOR_BSD 1 -#include - -/* - * User Datagram Header 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 - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Source Port | Destination Port | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Length | Checksum | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Data | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - */ - -/****************************************************************************** - * get - ******************************************************************************/ - -static inline uint16_t udp_hdr_get_src_port(const struct udphdr *hdr) -{ - return ntohs(hdr->uh_sport); -} - -static inline uint16_t udp_hdr_get_dst_port(const struct udphdr *hdr) -{ - return ntohs(hdr->uh_dport); -} - -static inline uint16_t udp_hdr_get_total_len(const struct udphdr *hdr) -{ - return ntohs(hdr->uh_ulen); -} - -static inline uint16_t udp_hdr_get_checksum(const struct udphdr *hdr) -{ - return ntohs(hdr->uh_sum); -} - -/****************************************************************************** - * set - ******************************************************************************/ - -static inline void udp_hdr_set_src_port(struct udphdr *hdr, uint16_t port) -{ - hdr->uh_sport = htons(port); -} - -static inline void udp_hdr_set_dst_port(struct udphdr *hdr, uint16_t port) -{ - hdr->uh_dport = htons(port); -} - -static inline void udp_hdr_set_total_len(struct udphdr *hdr, uint16_t len) -{ - hdr->uh_ulen = htons(len); -} - -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); - return snprintf(buf, size, "UDP: src_port=%u dst_port=%u total_len=%u checksum=0x%x", - udp_hdr_get_src_port(hdr), udp_hdr_get_dst_port(hdr), - udp_hdr_get_total_len(hdr), udp_hdr_get_checksum(hdr)); -} - -#ifdef __cplusplus -} -#endif diff --git a/src/packet/vlan_utils.h b/src/packet/vlan_utils.h deleted file mode 100644 index 69793a2..0000000 --- a/src/packet/vlan_utils.h +++ /dev/null @@ -1,92 +0,0 @@ -#pragma once - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include -#include -#include -#include "eth_utils.h" - -/* - * VLAN Header 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 - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | Pri |I| VLAN ID | Ethertype | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - */ - -struct vlan_hdr -{ - uint16_t vlan_tci; - uint16_t vlan_ethertype; -}; - -/****************************************************************************** - * get - ******************************************************************************/ - -static inline uint8_t vlan_hdr_get_priority(const struct vlan_hdr *hdr) -{ - return (ntohs(hdr->vlan_tci) & 0xe000) >> 13; -} - -static inline uint8_t vlan_hdr_get_dei(const struct vlan_hdr *hdr) -{ - return (ntohs(hdr->vlan_tci) & 0x1000) >> 12; -} - -static inline uint16_t vlan_hdr_get_vid(const struct vlan_hdr *hdr) -{ - return ntohs(hdr->vlan_tci) & 0x0fff; -} - -static inline uint16_t vlan_hdr_get_ethertype(const struct vlan_hdr *hdr) -{ - return ntohs(hdr->vlan_ethertype); -} - -/****************************************************************************** - * set - ******************************************************************************/ - -static inline void vlan_hdr_set_priority(struct vlan_hdr *hdr, uint8_t priority) -{ - hdr->vlan_tci = htons((ntohs(hdr->vlan_tci) & ~0xe000) | (priority << 13)); -} - -static inline void vlan_hdr_set_dei(struct vlan_hdr *hdr, uint8_t dei) -{ - hdr->vlan_tci = htons((ntohs(hdr->vlan_tci) & ~0x1000) | (dei << 12)); -} - -static inline void vlan_hdr_set_vid(struct vlan_hdr *hdr, uint16_t vid) -{ - hdr->vlan_tci = htons((ntohs(hdr->vlan_tci) & ~0x0fff) | vid); -} - -static inline void vlan_hdr_set_ethertype(struct vlan_hdr *hdr, uint16_t ethertype) -{ - hdr->vlan_ethertype = htons(ethertype); -} - -/****************************************************************************** - * print - ******************************************************************************/ - -static inline int vlan_hdr_to_str(const struct vlan_hdr *hdr, char *buf, size_t size) -{ - memset(buf, 0, size); - uint16_t proto = vlan_hdr_get_ethertype(hdr); - return snprintf(buf, size, "VLAN: priority=%u dei=%u id=%u ethertype=%s", - vlan_hdr_get_priority(hdr), vlan_hdr_get_dei(hdr), - vlan_hdr_get_vid(hdr), eth_proto_to_str(proto)); -} - -#ifdef __cplusplus -} -#endif diff --git a/src/packet/vxlan_utils.h b/src/packet/vxlan_utils.h deleted file mode 100644 index 125edd8..0000000 --- a/src/packet/vxlan_utils.h +++ /dev/null @@ -1,71 +0,0 @@ -#pragma once - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include -#include -#include - -/* - * VXLAN Header - * - * 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 - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * |R|R|R|R|I|R|R|R| Reserved | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * | VXLAN Network Identifier (VNI) | Reserved | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - */ - -struct vxlan_hdr -{ - uint32_t vxlan_flags; - uint32_t vxlan_vni; -}; - -/****************************************************************************** - * get - ******************************************************************************/ - -static inline uint8_t vxlan_hdr_get_flags(const struct vxlan_hdr *hdr) -{ - return (ntohl(hdr->vxlan_flags) & 0xff000000) >> 24; -} - -static inline uint32_t vxlan_hdr_get_vni(const struct vxlan_hdr *hdr) -{ - return (ntohl(hdr->vxlan_vni) & 0xffffff00) >> 8; -} - -/****************************************************************************** - * set - ******************************************************************************/ - -static inline void vxlan_hdr_set_flags(struct vxlan_hdr *hdr, uint8_t flags) -{ - hdr->vxlan_flags = htonl((ntohl(hdr->vxlan_flags) & ~0xff000000) | (flags << 24)); -} - -static inline void vxlan_hdr_set_vni(struct vxlan_hdr *hdr, uint32_t vni) -{ - hdr->vxlan_vni = htonl((ntohl(hdr->vxlan_vni) & ~0xffffff00) | (vni << 8)); -} - -/****************************************************************************** - * print - ******************************************************************************/ - -static inline int vxlan_hdr_to_str(const struct vxlan_hdr *hdr, char *buf, size_t size) -{ - memset(buf, 0, size); - return snprintf(buf, size, "VXLAN: flags=%u vni=%u", - vxlan_hdr_get_flags(hdr), vxlan_hdr_get_vni(hdr)); -} - -#ifdef __cplusplus -} -#endif diff --git a/src/session/session_manager.cpp b/src/session/session_manager.cpp index ba28165..70c9202 100644 --- a/src/session/session_manager.cpp +++ b/src/session/session_manager.cpp @@ -4,8 +4,7 @@ #include "log.h" #include "utils.h" -#include "tcp_utils.h" -#include "udp_utils.h" +#include "packet_helper.h" #include "packet_layer.h" #include "packet_utils.h" #include "packet_filter.h" diff --git a/src/session/test/gtest_filter_tcp_dupkt.cpp b/src/session/test/gtest_filter_tcp_dupkt.cpp index 967056e..3e4bf1d 100644 --- a/src/session/test/gtest_filter_tcp_dupkt.cpp +++ b/src/session/test/gtest_filter_tcp_dupkt.cpp @@ -1,6 +1,6 @@ #include -#include "ip4_utils.h" +#include "packet_helper.h" #include "packet_private.h" #include "packet_parser.h" #include "packet_layer.h" diff --git a/src/session/test/gtest_overload_evict_tcp_sess.cpp b/src/session/test/gtest_overload_evict_tcp_sess.cpp index 38a277a..4f3f5a7 100644 --- a/src/session/test/gtest_overload_evict_tcp_sess.cpp +++ b/src/session/test/gtest_overload_evict_tcp_sess.cpp @@ -1,7 +1,7 @@ #include #include "utils.h" -#include "ip4_utils.h" +#include "packet_helper.h" #include "packet_private.h" #include "packet_parser.h" #include "packet_layer.h" diff --git a/src/session/test/gtest_overload_evict_udp_sess.cpp b/src/session/test/gtest_overload_evict_udp_sess.cpp index ca45243..ecc98e4 100644 --- a/src/session/test/gtest_overload_evict_udp_sess.cpp +++ b/src/session/test/gtest_overload_evict_udp_sess.cpp @@ -1,7 +1,7 @@ #include #include "utils.h" -#include "ip4_utils.h" +#include "packet_helper.h" #include "packet_private.h" #include "packet_parser.h" #include "packet_layer.h" diff --git a/src/session/test/gtest_sess_mgr_tcp_reassembly.cpp b/src/session/test/gtest_sess_mgr_tcp_reassembly.cpp index ce4455d..439fd72 100644 --- a/src/session/test/gtest_sess_mgr_tcp_reassembly.cpp +++ b/src/session/test/gtest_sess_mgr_tcp_reassembly.cpp @@ -1,6 +1,6 @@ #include -#include "ip4_utils.h" +#include "packet_helper.h" #include "packet_private.h" #include "packet_parser.h" #include "packet_layer.h" diff --git a/src/session/test/gtest_state_tcp_active_to_closing.cpp b/src/session/test/gtest_state_tcp_active_to_closing.cpp index a667f2f..8c874e5 100644 --- a/src/session/test/gtest_state_tcp_active_to_closing.cpp +++ b/src/session/test/gtest_state_tcp_active_to_closing.cpp @@ -2,7 +2,7 @@ #include #include "tuple.h" -#include "tcp_utils.h" +#include "packet_helper.h" #include "packet_private.h" #include "packet_parser.h" #include "packet_layer.h" diff --git a/src/session/test/gtest_state_tcp_init_to_opening.cpp b/src/session/test/gtest_state_tcp_init_to_opening.cpp index 8b84982..66949ed 100644 --- a/src/session/test/gtest_state_tcp_init_to_opening.cpp +++ b/src/session/test/gtest_state_tcp_init_to_opening.cpp @@ -2,7 +2,7 @@ #include #include "tuple.h" -#include "ip4_utils.h" +#include "packet_helper.h" #include "packet_private.h" #include "packet_parser.h" #include "packet_layer.h" diff --git a/src/session/test/gtest_state_tcp_opening_to_closing.cpp b/src/session/test/gtest_state_tcp_opening_to_closing.cpp index d3bcd56..5ca5023 100644 --- a/src/session/test/gtest_state_tcp_opening_to_closing.cpp +++ b/src/session/test/gtest_state_tcp_opening_to_closing.cpp @@ -2,7 +2,7 @@ #include #include "tuple.h" -#include "tcp_utils.h" +#include "packet_helper.h" #include "packet_private.h" #include "packet_parser.h" #include "packet_layer.h" diff --git a/test/packet_tool/packet_tool.cpp b/test/packet_tool/packet_tool.cpp index 4e1f255..420cd45 100644 --- a/test/packet_tool/packet_tool.cpp +++ b/test/packet_tool/packet_tool.cpp @@ -1,12 +1,7 @@ #include #include -#include "eth_utils.h" -#include "vlan_utils.h" -#include "ip4_utils.h" -#include "ip6_utils.h" -#include "tcp_utils.h" -#include "udp_utils.h" +#include "packet_helper.h" #include "packet_private.h" #include "packet_layer.h" #include "packet_parser.h"