diff --git a/src/packet/gtp1_utils.h b/src/packet/gtp1_utils.h index db4ed2b..3ca4235 100644 --- a/src/packet/gtp1_utils.h +++ b/src/packet/gtp1_utils.h @@ -99,7 +99,7 @@ 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_protocol(const struct gtp1_hdr *gtp) +static inline uint8_t gtp1_hdr_get_proto(const struct gtp1_hdr *gtp) { return (gtp->flags & GTP1_FLAG_PROTOCOL) >> 4; } @@ -178,15 +178,6 @@ static inline uint8_t gtp1_hdr_get_next_ext_type(const struct gtp1_hdr *gtp) } } -static inline uint8_t peek_gtp_version(const char *data, uint16_t len) -{ - if (data == NULL || len == 0) - { - return 0; - } - return ((*(uint8_t *)data) >> 5) & 0x07; -} - // include gtp fixed header and optional headers and extension headers static inline uint16_t calc_gtp1_hdr_len(const char *data, uint16_t len) { @@ -248,7 +239,7 @@ 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_protocol(struct gtp1_hdr *gtp, uint8_t protocol_type) +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); } @@ -321,11 +312,13 @@ static inline void gtp1_hdr_set_next_ext_type(struct gtp1_hdr *gtp, uint8_t next 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_protocol(gtp), + gtp1_hdr_get_proto(gtp), gtp1_hdr_get_reserved(gtp), gtp1_hdr_get_ext_flag(gtp), gtp1_hdr_get_seq_flag(gtp), diff --git a/src/packet/gtp2_utils.h b/src/packet/gtp2_utils.h index 41661f6..c7e6cf5 100644 --- a/src/packet/gtp2_utils.h +++ b/src/packet/gtp2_utils.h @@ -242,6 +242,8 @@ static inline void gtp2_hdr_set_spare(struct gtp2_hdr *gtp, uint8_t 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), diff --git a/src/packet/gtp_utils.h b/src/packet/gtp_utils.h new file mode 100644 index 0000000..8b0fc00 --- /dev/null +++ b/src/packet/gtp_utils.h @@ -0,0 +1,51 @@ +#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/packet_build.cpp b/src/packet/packet_build.cpp index a3609b1..ead36b0 100644 --- a/src/packet/packet_build.cpp +++ b/src/packet/packet_build.cpp @@ -6,8 +6,7 @@ #include "udp_utils.h" #include "ip4_utils.h" #include "ip6_utils.h" -#include "gtp1_utils.h" -#include "gtp2_utils.h" +#include "gtp_utils.h" #include "packet_def.h" #include "packet_utils.h" #include "packet_layer.h" diff --git a/src/packet/packet_parse.cpp b/src/packet/packet_parse.cpp index b52e8b7..88fc6a5 100644 --- a/src/packet/packet_parse.cpp +++ b/src/packet/packet_parse.cpp @@ -11,8 +11,7 @@ #include "tcp_utils.h" #include "ip4_utils.h" #include "ip6_utils.h" -#include "gtp1_utils.h" -#include "gtp2_utils.h" +#include "gtp_utils.h" #include "mpls_utils.h" #include "l2tp_utils.h" #include "vlan_utils.h" @@ -689,7 +688,7 @@ static inline const char *parse_gre(struct packet *pkt, const char *data, uint16 { return data; } - uint16_t next_proto = gre_hdr_get_proto((const struct gre_hdr *)data); + uint16_t next_proto = peek_gre_proto(data, len); SET_LAYER(pkt, layer, LAYER_PROTO_GRE, hdr_len, data, len, 0); return parse_l3(pkt, next_proto, layer->pld_ptr, layer->pld_len); @@ -856,7 +855,7 @@ static inline const char *parse_gtp_u(struct packet *pkt, const char *data, uint return data; } - uint16_t hdr_len = calc_gtp1_hdr_len(data, len); + uint16_t hdr_len = calc_gtp_hdr_len(data, len); if (unlikely(hdr_len == 0 || hdr_len > len)) { PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_GTP_U); @@ -886,20 +885,7 @@ static inline const char *parse_gtp_u(struct packet *pkt, const char *data, uint static inline const char *parse_gtp_c(struct packet *pkt, const char *data, uint16_t len) { // GTPv1-C or GTPv2-C - uint16_t hdr_len = 0; - uint8_t version = peek_gtp_version(data, len); - switch (version) - { - case 1: - hdr_len = calc_gtp1_hdr_len(data, len); - break; - case 2: - hdr_len = calc_gtp2_hdr_len(data, len); - break; - default: - return data; - } - + uint16_t hdr_len = calc_gtp_hdr_len(data, len); if (unlikely(hdr_len == 0 || hdr_len > len)) { PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_GTP_C); @@ -1033,7 +1019,7 @@ void packet_print(const struct packet *pkt) case LAYER_PROTO_IPAH: break; case LAYER_PROTO_GRE: - used = gre_hdr_to_str((const struct gre_hdr *)layer->hdr_ptr, buffer, sizeof(buffer)); + used = gre_hdr_to_str(layer->hdr_ptr, layer->hdr_len, buffer, sizeof(buffer)); break; case LAYER_PROTO_UDP: used = udp_hdr_to_str((const struct udphdr *)layer->hdr_ptr, buffer, sizeof(buffer)); @@ -1050,14 +1036,7 @@ void packet_print(const struct packet *pkt) break; case LAYER_PROTO_GTP_C: case LAYER_PROTO_GTP_U: - if (peek_gtp_version(layer->hdr_ptr, layer->hdr_len) == 1) - { - used = gtp1_hdr_to_str((const struct gtp1_hdr *)layer->hdr_ptr, buffer, sizeof(buffer)); - } - if (peek_gtp_version(layer->hdr_ptr, layer->hdr_len) == 2) - { - used = gtp2_hdr_to_str((const struct gtp2_hdr *)layer->hdr_ptr, buffer, sizeof(buffer)); - } + used = gtp_hdr_to_str(layer->hdr_ptr, layer->hdr_len, buffer, sizeof(buffer)); break; default: break; diff --git a/src/packet/test/gtest_gtp1_utils.cpp b/src/packet/test/gtest_gtp1_utils.cpp index 8d917e8..231dd1c 100644 --- a/src/packet/test/gtest_gtp1_utils.cpp +++ b/src/packet/test/gtest_gtp1_utils.cpp @@ -132,7 +132,7 @@ TEST(GTP1_UTILS, C_GET) EXPECT_TRUE(calc_gtp1_hdr_len((const char *)gtp1_c, sizeof(gtp1_c)) == 12); EXPECT_TRUE(gtp1_hdr_get_flags(hdr) == 0x32); EXPECT_TRUE(gtp1_hdr_get_version(hdr) == 1); - EXPECT_TRUE(gtp1_hdr_get_protocol(hdr) == 1); + EXPECT_TRUE(gtp1_hdr_get_proto(hdr) == 1); EXPECT_TRUE(gtp1_hdr_get_reserved(hdr) == 0); EXPECT_TRUE(gtp1_hdr_get_ext_flag(hdr) == 0); EXPECT_TRUE(gtp1_hdr_get_seq_flag(hdr) == 1); @@ -156,7 +156,7 @@ TEST(GTP1_UTILS, C_SET) struct gtp1_hdr *hdr = (struct gtp1_hdr *)buff; gtp1_hdr_set_flags(hdr, 0x32); gtp1_hdr_set_version(hdr, 1); - gtp1_hdr_set_protocol(hdr, 1); + gtp1_hdr_set_proto(hdr, 1); gtp1_hdr_set_reserved(hdr, 0); gtp1_hdr_set_ext_flag(hdr, 0); gtp1_hdr_set_seq_flag(hdr, 1); @@ -197,7 +197,7 @@ TEST(GTP1_UTILS, U_GET) EXPECT_TRUE(calc_gtp1_hdr_len((const char *)gtp1_u, sizeof(gtp1_u)) == 8); EXPECT_TRUE(gtp1_hdr_get_flags(hdr) == 0x30); EXPECT_TRUE(gtp1_hdr_get_version(hdr) == 1); - EXPECT_TRUE(gtp1_hdr_get_protocol(hdr) == 1); + EXPECT_TRUE(gtp1_hdr_get_proto(hdr) == 1); EXPECT_TRUE(gtp1_hdr_get_reserved(hdr) == 0); EXPECT_TRUE(gtp1_hdr_get_ext_flag(hdr) == 0); EXPECT_TRUE(gtp1_hdr_get_seq_flag(hdr) == 0); @@ -221,7 +221,7 @@ TEST(GTP1_UTILS, U_SET) struct gtp1_hdr *hdr = (struct gtp1_hdr *)buff; gtp1_hdr_set_flags(hdr, 0x30); gtp1_hdr_set_version(hdr, 1); - gtp1_hdr_set_protocol(hdr, 1); + gtp1_hdr_set_proto(hdr, 1); gtp1_hdr_set_reserved(hdr, 0); gtp1_hdr_set_ext_flag(hdr, 0); gtp1_hdr_set_seq_flag(hdr, 0);