diff --git a/src/packet/eth_utils.h b/src/packet/eth_utils.h index 63c5d68..3a64db7 100644 --- a/src/packet/eth_utils.h +++ b/src/packet/eth_utils.h @@ -88,6 +88,7 @@ static inline int is_eth_proto(uint16_t proto) 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: @@ -186,6 +187,8 @@ static inline const char *eth_proto_to_str(uint16_t proto) 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: diff --git a/src/packet/ipv6_utils.h b/src/packet/ipv6_utils.h index 82b4569..b7e9cc2 100644 --- a/src/packet/ipv6_utils.h +++ b/src/packet/ipv6_utils.h @@ -40,13 +40,13 @@ extern "C" * * 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 | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * 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 | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ */ /****************************************************************************** diff --git a/src/packet/mpls_utils.h b/src/packet/mpls_utils.h index 5ff3352..c6571d8 100644 --- a/src/packet/mpls_utils.h +++ b/src/packet/mpls_utils.h @@ -11,18 +11,14 @@ extern "C" #include #include -/* Reference: RFC 5462, RFC 3032 +/* + * 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 | - * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ - * - * Label: Label Value, 20 bits - * TC: Traffic Class, 3 bits - * S: Bottom of Stack, 1 bit - * TTL: Time to Live, 8 bits + * 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 | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ */ /****************************************************************************** diff --git a/src/packet/packet.cpp b/src/packet/packet.cpp index b750d44..b8b4035 100644 --- a/src/packet/packet.cpp +++ b/src/packet/packet.cpp @@ -12,6 +12,7 @@ #include "ipv6_utils.h" #include "mpls_utils.h" #include "eth_utils.h" +#include "vlan_utils.h" #define likely(expr) __builtin_expect((expr), 1) #define unlikely(expr) __builtin_expect((expr), 0) @@ -772,12 +773,6 @@ static inline const char *parse_l2tpv3_over_ip(struct packet *pkt, const char *d static inline const char *parse_vlan(struct packet *pkt, const char *data, uint16_t len) { - struct vlan_hdr - { - uint16_t vlan_cfi; - uint16_t protocol; - } __attribute__((__packed__)); - if (unlikely(len < sizeof(struct vlan_hdr))) { PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_TYPE_VLAN); @@ -789,7 +784,7 @@ static inline const char *parse_vlan(struct packet *pkt, const char *data, uint1 { return data; } - uint16_t next_proto = ntohs(((struct vlan_hdr *)data)->protocol); + uint16_t next_proto = vlan_hdr_get_ethertype((const struct vlan_hdr *)data); SET_LAYER(pkt, layer, LAYER_TYPE_VLAN, sizeof(struct vlan_hdr), data, len, 0); return parse_l3(pkt, next_proto, layer->pld_ptr, layer->pld_len); @@ -1379,6 +1374,7 @@ void packet_print_str(const struct packet *pkt) case LAYER_TYPE_L2TP: break; case LAYER_TYPE_VLAN: + used = vlan_hdr_to_str((const struct vlan_hdr *)layer->hdr_ptr, buffer, sizeof(buffer)); break; case LAYER_TYPE_PPPOE: break; diff --git a/src/packet/test/CMakeLists.txt b/src/packet/test/CMakeLists.txt index cc9461c..c0bb9da 100644 --- a/src/packet/test/CMakeLists.txt +++ b/src/packet/test/CMakeLists.txt @@ -19,6 +19,9 @@ target_link_libraries(gtest_mpls_utils packet gtest) add_executable(gtest_eth_utils gtest_eth_utils.cpp) target_link_libraries(gtest_eth_utils packet gtest) +add_executable(gtest_vlan_utils gtest_vlan_utils.cpp) +target_link_libraries(gtest_vlan_utils packet gtest) + add_executable(gtest_packet_frag gtest_packet_frag.cpp) target_link_libraries(gtest_packet_frag packet gtest) @@ -30,4 +33,5 @@ gtest_discover_tests(gtest_ipv4_utils) gtest_discover_tests(gtest_ipv6_utils) gtest_discover_tests(gtest_mpls_utils) gtest_discover_tests(gtest_eth_utils) +gtest_discover_tests(gtest_vlan_utils) gtest_discover_tests(gtest_packet_frag) \ No newline at end of file diff --git a/src/packet/test/gtest_vlan_utils.cpp b/src/packet/test/gtest_vlan_utils.cpp new file mode 100644 index 0000000..1506f69 --- /dev/null +++ b/src/packet/test/gtest_vlan_utils.cpp @@ -0,0 +1,42 @@ +#include + +#include "vlan_utils.h" + +/* + * IEEE 802.1ad, ID: 1 + * 000. .... .... .... = Priority: 0 + * ...0 .... .... .... = DEI: 0 + * .... 0000 0000 0001 = ID: 1 + * Type: 802.1Q Virtual LAN (0x8100) + */ + +unsigned char data[] = { + 0x00, 0x01, 0x81, 0x00}; + +TEST(VLAN_UTILS, GET) +{ + const struct vlan_hdr *hdr = (struct vlan_hdr *)data; + + EXPECT_TRUE(vlan_hdr_get_priority(hdr) == 0); + EXPECT_TRUE(vlan_hdr_get_dei(hdr) == 0); + EXPECT_TRUE(vlan_hdr_get_vid(hdr) == 1); + EXPECT_TRUE(vlan_hdr_get_ethertype(hdr) == 0x8100); +} + +TEST(VLAN_UTILS, SET) +{ + char buff[4] = {0}; + struct vlan_hdr *hdr = (struct vlan_hdr *)buff; + + vlan_hdr_set_priority(hdr, 0); + vlan_hdr_set_dei(hdr, 0); + vlan_hdr_set_vid(hdr, 1); + vlan_hdr_set_ethertype(hdr, 0x8100); + EXPECT_TRUE(memcmp(buff, data, 4) == 0); +} + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/packet/vlan_utils.h b/src/packet/vlan_utils.h new file mode 100644 index 0000000..5cc52bb --- /dev/null +++ b/src/packet/vlan_utils.h @@ -0,0 +1,95 @@ +#ifndef _VLAN_UTILS_H +#define _VLAN_UTILS_H + +#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 + +#endif diff --git a/test/packet_parser/cmp_layers.sh b/test/packet_parser/cmp_layers.sh index d61a188..28d4729 100644 --- a/test/packet_parser/cmp_layers.sh +++ b/test/packet_parser/cmp_layers.sh @@ -155,6 +155,7 @@ for pcap in "${pcap_files[@]}"; do -e frame.protocols \ -e eth.src \ -e eth.dst \ + -e vlan.id \ -e ip.src \ -e ip.dst \ -e ipv6.src \ diff --git a/test/packet_parser/packet_parser.cpp b/test/packet_parser/packet_parser.cpp index 0d3d04a..a72c747 100644 --- a/test/packet_parser/packet_parser.cpp +++ b/test/packet_parser/packet_parser.cpp @@ -3,6 +3,7 @@ #include "ipv6_utils.h" #include "packet_priv.h" #include "eth_utils.h" +#include "vlan_utils.h" #include "ipv4_utils.h" #include "ipv6_utils.h" #include "tcp_utils.h" @@ -48,6 +49,7 @@ static void packet_to_tshark_format(const struct packet *pkt, uint64_t idx) -e frame.protocols \ -e eth.src \ -e eth.dst \ + -e vlan.id \ -e ip.src \ -e ip.dst \ -e ipv6.src \ @@ -62,6 +64,7 @@ static void packet_to_tshark_format(const struct packet *pkt, uint64_t idx) struct buffer buff_proto = {.buff = {0}, .used = 0, .elimiter = ':'}; struct buffer buff_eth_src = {.buff = {0}, .used = 0, .elimiter = ','}; struct buffer buff_eth_dst = {.buff = {0}, .used = 0, .elimiter = ','}; + struct buffer buff_vlan_id = {.buff = {0}, .used = 0, .elimiter = ','}; struct buffer buff_ipv4_src = {.buff = {0}, .used = 0, .elimiter = ','}; struct buffer buff_ipv4_dst = {.buff = {0}, .used = 0, .elimiter = ','}; struct buffer buff_ipv6_src = {.buff = {0}, .used = 0, .elimiter = ','}; @@ -71,6 +74,7 @@ static void packet_to_tshark_format(const struct packet *pkt, uint64_t idx) struct buffer buff_udp_src = {.buff = {0}, .used = 0, .elimiter = ','}; struct buffer buff_udp_dst = {.buff = {0}, .used = 0, .elimiter = ','}; const struct ethhdr *eth_hdr = NULL; + const struct vlan_hdr *vlan_hdr = NULL; const struct ip *ipv4_hdr = NULL; const struct ip6_hdr *ipv6_hdr = NULL; const struct tcphdr *tcp_hdr = NULL; @@ -81,6 +85,7 @@ static void packet_to_tshark_format(const struct packet *pkt, uint64_t idx) struct in6_addr dst_addr_v6 = {0}; uint16_t src_port = 0; uint16_t dst_port = 0; + uint16_t vlan_id = 0; char tmp_src_buff[256] = {0}; char tmp_dst_buff[256] = {0}; @@ -114,6 +119,10 @@ static void packet_to_tshark_format(const struct packet *pkt, uint64_t idx) break; case LAYER_TYPE_VLAN: buffer_push(&buff_proto, "vlan:ethertype"); + vlan_hdr = (const struct vlan_hdr *)layer->hdr_ptr; + vlan_id = vlan_hdr_get_vid(vlan_hdr); + snprintf(tmp_src_buff, sizeof(tmp_src_buff), "%u", vlan_id); + buffer_push(&buff_vlan_id, tmp_src_buff); break; case LAYER_TYPE_PPPOE: buffer_push(&buff_proto, "pppoes"); @@ -205,11 +214,12 @@ static void packet_to_tshark_format(const struct packet *pkt, uint64_t idx) } } - printf("%lu\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n", + printf("%lu\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n", number, buff_proto.buff, buff_eth_src.buff, buff_eth_dst.buff, + buff_vlan_id.buff, buff_ipv4_src.buff, buff_ipv4_dst.buff, buff_ipv6_src.buff,