diff --git a/qdf/inc/qdf_nbuf.h b/qdf/inc/qdf_nbuf.h index 4ab58f08f2..5570b8ea91 100644 --- a/qdf/inc/qdf_nbuf.h +++ b/qdf/inc/qdf_nbuf.h @@ -51,6 +51,8 @@ #define QDF_NBUF_TRAC_DHCP_SRV_PORT 67 #define QDF_NBUF_TRAC_DHCP_CLI_PORT 68 #define QDF_NBUF_TRAC_ETH_TYPE_OFFSET 12 +#define QDF_NBUF_TRAC_VLAN_ETH_TYPE_OFFSET 16 +#define QDF_NBUF_TRAC_DOUBLE_VLAN_ETH_TYPE_OFFSET 20 #define QDF_NBUF_TRAC_EAPOL_ETH_TYPE 0x888E #define QDF_NBUF_TRAC_WAPI_ETH_TYPE 0x88b4 #define QDF_NBUF_TRAC_ARP_ETH_TYPE 0x0806 @@ -84,7 +86,11 @@ #define QDF_NBUF_TRAC_DHCP6_SRV_PORT 547 #define QDF_NBUF_TRAC_DHCP6_CLI_PORT 546 #define QDF_NBUF_TRAC_MDNS_SRC_N_DST_PORT 5353 - +#define QDF_NBUF_TRAC_IP_OFFSET 14 +#define QDF_NBUF_TRAC_VLAN_IP_OFFSET 18 +#define QDF_NBUF_TRAC_DOUBLE_VLAN_IP_OFFSET 22 +/* One dword for IPv4 header size unit */ +#define QDF_NBUF_IPV4_HDR_SIZE_UNIT 4 /* EAPOL Related MASK */ #define EAPOL_PACKET_TYPE_OFFSET 15 diff --git a/qdf/inc/qdf_net_types.h b/qdf/inc/qdf_net_types.h index edebc8ec6a..c3a60736bb 100644 --- a/qdf/inc/qdf_net_types.h +++ b/qdf/inc/qdf_net_types.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2014-2019 The Linux Foundation. All rights reserved. + * Copyright (c) 2014-2020 The Linux Foundation. All rights reserved. * * Permission to use, copy, modify, and/or distribute this software for * any purpose with or without fee is hereby granted, provided that the @@ -61,6 +61,7 @@ typedef __in6_addr_t in6_addr_t; #define QDF_ETH_TYPE_IPV4 0x0800 /* IPV4 */ #define QDF_ETH_TYPE_IPV6 0x86dd /* IPV6 */ #define QDF_ETH_TYPE_8021Q 0x8100 /* 802.1Q vlan protocol */ +#define QDF_ETH_TYPE_8021AD 0x88a8 /* 802.1AD vlan protocol */ #define QDF_IEEE80211_4ADDR_HDR_LEN 30 #define QDF_IEEE80211_3ADDR_HDR_LEN 24 #define QDF_IEEE80211_FC0_SUBTYPE_QOS 0x80 diff --git a/qdf/linux/src/qdf_nbuf.c b/qdf/linux/src/qdf_nbuf.c index 70800ba044..3a86f026fe 100644 --- a/qdf/linux/src/qdf_nbuf.c +++ b/qdf/linux/src/qdf_nbuf.c @@ -118,6 +118,61 @@ static qdf_atomic_t nbuf_count; static bool is_initial_mem_debug_disabled; #endif +/** + * __qdf_nbuf_get_ip_offset - Get IPV4/V6 header offset + * @data: Pointer to network data buffer + * + * Get the IP header offset in case of 8021Q and 8021AD + * tag is present in L2 header. + * + * Return: IP header offset + */ +static inline uint8_t __qdf_nbuf_get_ip_offset(uint8_t *data) +{ + uint16_t ether_type; + + ether_type = *(uint16_t *)(data + + QDF_NBUF_TRAC_ETH_TYPE_OFFSET); + + if (unlikely(ether_type == QDF_SWAP_U16(QDF_ETH_TYPE_8021Q))) + return QDF_NBUF_TRAC_VLAN_IP_OFFSET; + else if (unlikely(ether_type == QDF_SWAP_U16(QDF_ETH_TYPE_8021AD))) + return QDF_NBUF_TRAC_DOUBLE_VLAN_IP_OFFSET; + + return QDF_NBUF_TRAC_IP_OFFSET; +} + +qdf_export_symbol(__qdf_nbuf_get_ip_offset); + +/** + * __qdf_nbuf_get_ether_type - Get the ether type + * @data: Pointer to network data buffer + * + * Get the ether type in case of 8021Q and 8021AD tag + * is present in L2 header, e.g for the returned ether type + * value, if IPV4 data ether type 0x0800, return 0x0008. + * + * Return ether type. + */ +static inline uint16_t __qdf_nbuf_get_ether_type(uint8_t *data) +{ + uint16_t ether_type; + + ether_type = *(uint16_t *)(data + + QDF_NBUF_TRAC_ETH_TYPE_OFFSET); + + if (unlikely(ether_type == QDF_SWAP_U16(QDF_ETH_TYPE_8021Q))) + ether_type = *(uint16_t *)(data + + QDF_NBUF_TRAC_VLAN_ETH_TYPE_OFFSET); + else if (unlikely(ether_type == QDF_SWAP_U16(QDF_ETH_TYPE_8021AD))) + ether_type = *(uint16_t *)(data + + QDF_NBUF_TRAC_DOUBLE_VLAN_ETH_TYPE_OFFSET); + + return ether_type; +} + +qdf_export_symbol(__qdf_nbuf_get_ether_type); + /** * qdf_nbuf_tx_desc_count_display() - Displays the packet counter * @@ -1370,12 +1425,21 @@ bool __qdf_nbuf_data_is_ipv4_dhcp_pkt(uint8_t *data) { uint16_t sport; uint16_t dport; + uint8_t ipv4_offset; + uint8_t ipv4_hdr_len; + struct iphdr *iphdr; - sport = (uint16_t)(*(uint16_t *)(data + QDF_NBUF_TRAC_IPV4_OFFSET + - QDF_NBUF_TRAC_IPV4_HEADER_SIZE)); - dport = (uint16_t)(*(uint16_t *)(data + QDF_NBUF_TRAC_IPV4_OFFSET + - QDF_NBUF_TRAC_IPV4_HEADER_SIZE + - sizeof(uint16_t))); + if (__qdf_nbuf_get_ether_type(data) != + QDF_SWAP_U16(QDF_NBUF_TRAC_IPV4_ETH_TYPE)) + return false; + + ipv4_offset = __qdf_nbuf_get_ip_offset(data); + iphdr = (struct iphdr *)(data + ipv4_offset); + ipv4_hdr_len = iphdr->ihl * QDF_NBUF_IPV4_HDR_SIZE_UNIT; + + sport = *(uint16_t *)(data + ipv4_offset + ipv4_hdr_len); + dport = *(uint16_t *)(data + ipv4_offset + ipv4_hdr_len + + sizeof(uint16_t)); if (((sport == QDF_SWAP_U16(QDF_NBUF_TRAC_DHCP_SRV_PORT)) && (dport == QDF_SWAP_U16(QDF_NBUF_TRAC_DHCP_CLI_PORT))) || @@ -1400,8 +1464,7 @@ bool __qdf_nbuf_data_is_ipv4_eapol_pkt(uint8_t *data) { uint16_t ether_type; - ether_type = (uint16_t)(*(uint16_t *)(data + - QDF_NBUF_TRAC_ETH_TYPE_OFFSET)); + ether_type = __qdf_nbuf_get_ether_type(data); if (ether_type == QDF_SWAP_U16(QDF_NBUF_TRAC_EAPOL_ETH_TYPE)) return true; @@ -1469,8 +1532,7 @@ bool __qdf_nbuf_data_is_ipv4_arp_pkt(uint8_t *data) { uint16_t ether_type; - ether_type = (uint16_t)(*(uint16_t *)(data + - QDF_NBUF_TRAC_ETH_TYPE_OFFSET)); + ether_type = __qdf_nbuf_get_ether_type(data); if (ether_type == QDF_SWAP_U16(QDF_NBUF_TRAC_ARP_ETH_TYPE)) return true; @@ -1845,12 +1907,14 @@ bool __qdf_nbuf_data_is_ipv6_dhcp_pkt(uint8_t *data) { uint16_t sport; uint16_t dport; + uint8_t ipv6_offset; - sport = *(uint16_t *)(data + QDF_NBUF_TRAC_IPV6_OFFSET + - QDF_NBUF_TRAC_IPV6_HEADER_SIZE); - dport = *(uint16_t *)(data + QDF_NBUF_TRAC_IPV6_OFFSET + - QDF_NBUF_TRAC_IPV6_HEADER_SIZE + - sizeof(uint16_t)); + ipv6_offset = __qdf_nbuf_get_ip_offset(data); + sport = *(uint16_t *)(data + ipv6_offset + + QDF_NBUF_TRAC_IPV6_HEADER_SIZE); + dport = *(uint16_t *)(data + ipv6_offset + + QDF_NBUF_TRAC_IPV6_HEADER_SIZE + + sizeof(uint16_t)); if (((sport == QDF_SWAP_U16(QDF_NBUF_TRAC_DHCP6_SRV_PORT)) && (dport == QDF_SWAP_U16(QDF_NBUF_TRAC_DHCP6_CLI_PORT))) ||