packet: deliver VLAN TPID to userspace

This enables userspace to get VLAN TPID as well as the VLAN TCI.

Signed-off-by: Atzm Watanabe <atzm@stratosphere.co.jp>
Acked-by: Daniel Borkmann <dborkman@redhat.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

2 files changed, 24 insertions, 14 deletions

diff --git a/include/uapi/linux/if_packet.h b/include/uapi/linux/if_packet.h
index 9185dc9a4485..e9d844c80c11 100644
--- a/include/uapi/linux/if_packet.h
+++ b/include/uapi/linux/if_packet.h
@@ -84,17 +84,18 @@ struct tpacket_auxdata {
         __u16           tp_mac;
         __u16           tp_net;
         __u16           tp_vlan_tci;
-        __u16           tp_padding;
+        __u16           tp_vlan_tpid;
 };
 
 /* Rx ring - header status */
-#define TP_STATUS_KERNEL              0
-#define TP_STATUS_USER          (1 << 0)
-#define TP_STATUS_COPY          (1 << 1)
-#define TP_STATUS_LOSING        (1 << 2)
-#define TP_STATUS_CSUMNOTREADY  (1 << 3)
-#define TP_STATUS_VLAN_VALID    (1 << 4) /* auxdata has valid tp_vlan_tci */
-#define TP_STATUS_BLK_TMO       (1 << 5)
+#define TP_STATUS_KERNEL                      0
+#define TP_STATUS_USER                  (1 << 0)
+#define TP_STATUS_COPY                  (1 << 1)
+#define TP_STATUS_LOSING                (1 << 2)
+#define TP_STATUS_CSUMNOTREADY          (1 << 3)
+#define TP_STATUS_VLAN_VALID            (1 << 4) /* auxdata has valid tp_vlan_tci */
+#define TP_STATUS_BLK_TMO               (1 << 5)
+#define TP_STATUS_VLAN_TPID_VALID       (1 << 6) /* auxdata has valid tp_vlan_tpid */
 
 /* Tx ring - header status */
 #define TP_STATUS_AVAILABLE           0
@@ -133,12 +134,15 @@ struct tpacket2_hdr {
         __u32           tp_sec;
         __u32           tp_nsec;
         __u16           tp_vlan_tci;
-        __u8            tp_padding[6];
+        __u16           tp_vlan_tpid;
+        __u8            tp_padding[4];
 };
 
 struct tpacket_hdr_variant1 {
         __u32   tp_rxhash;
         __u32   tp_vlan_tci;
+        __u16   tp_vlan_tpid;
+        __u16   tp_padding;
 };
 
 struct tpacket3_hdr {
@@ -154,7 +158,7 @@ struct tpacket3_hdr {
         union {
                 struct tpacket_hdr_variant1 hv1;
         };
-        __u8            tp_padding[12];
+        __u8            tp_padding[8];
 };
 
 struct tpacket_bd_ts {
diff --git a/net/packet/af_packet.c b/net/packet/af_packet.c
index f281999ba92e..dd3840846ce2 100644
--- a/net/packet/af_packet.c
+++ b/net/packet/af_packet.c
@@ -977,9 +977,11 @@ static void prb_fill_vlan_info(struct tpacket_kbdq_core *pkc,
 {
         if (vlan_tx_tag_present(pkc->skb)) {
                 ppd->hv1.tp_vlan_tci = vlan_tx_tag_get(pkc->skb);
-                ppd->tp_status = TP_STATUS_VLAN_VALID;
+                ppd->hv1.tp_vlan_tpid = ntohs(pkc->skb->vlan_proto);
+                ppd->tp_status = TP_STATUS_VLAN_VALID | TP_STATUS_VLAN_TPID_VALID;
         } else {
                 ppd->hv1.tp_vlan_tci = 0;
+                ppd->hv1.tp_vlan_tpid = 0;
                 ppd->tp_status = TP_STATUS_AVAILABLE;
         }
 }
@@ -987,6 +989,7 @@ static void prb_fill_vlan_info(struct tpacket_kbdq_core *pkc,
 static void prb_run_all_ft_ops(struct tpacket_kbdq_core *pkc,
                         struct tpacket3_hdr *ppd)
 {
+        ppd->hv1.tp_padding = 0;
         prb_fill_vlan_info(pkc, ppd);
 
         if (pkc->feature_req_word & TP_FT_REQ_FILL_RXHASH)
@@ -1925,9 +1928,11 @@ static int tpacket_rcv(struct sk_buff *skb, struct net_device *dev,
                 h.h2->tp_nsec = ts.tv_nsec;
                 if (vlan_tx_tag_present(skb)) {
                         h.h2->tp_vlan_tci = vlan_tx_tag_get(skb);
-                        status |= TP_STATUS_VLAN_VALID;
+                        h.h2->tp_vlan_tpid = ntohs(skb->vlan_proto);
+                        status |= TP_STATUS_VLAN_VALID | TP_STATUS_VLAN_TPID_VALID;
                 } else {
                         h.h2->tp_vlan_tci = 0;
+                        h.h2->tp_vlan_tpid = 0;
                 }
                 memset(h.h2->tp_padding, 0, sizeof(h.h2->tp_padding));
                 hdrlen = sizeof(*h.h2);
@@ -2875,11 +2880,12 @@ static int packet_recvmsg(struct kiocb *iocb, struct socket *sock,
                 aux.tp_net = skb_network_offset(skb);
                 if (vlan_tx_tag_present(skb)) {
                         aux.tp_vlan_tci = vlan_tx_tag_get(skb);
-                        aux.tp_status |= TP_STATUS_VLAN_VALID;
+                        aux.tp_vlan_tpid = ntohs(skb->vlan_proto);
+                        aux.tp_status |= TP_STATUS_VLAN_VALID | TP_STATUS_VLAN_TPID_VALID;
                 } else {
                         aux.tp_vlan_tci = 0;
+                        aux.tp_vlan_tpid = 0;
                 }
-                aux.tp_padding = 0;
                 put_cmsg(msg, SOL_PACKET, PACKET_AUXDATA, sizeof(aux), &aux);
         }