[PATCH v2] packet: Deliver VLAN TPID to userspace

[Atzm Watanabe <atzm@stratosphere.co.jp>]

After the 802.1AD support, userspace packet receivers (packet dumper,
software switch, and the like) need how to know VLAN TPID in order to
reconstruct original tagged frame.

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

struct tpacket3_hdr is aligned to a multiple of 16 bytes by TPACKET_ALIGN().
Its current size is 36 bytes, so it can grow safely until 48 bytes
without forcing userspace to call getsockopt(..., PACKET_HDRLEN, ...).

v2: separate struct tpacket_hdr_variant2 which includes a new member
    tp_vlan_tpid, and add BUILD_BUG_ON().  commented by Ben Hutchings.

Cc: Ben Hutchings <bhutchings@solarflare.com>
Signed-off-by: Atzm Watanabe <atzm@stratosphere.co.jp>
---
Sorry for the long delay since last discussions.
I've been blocked by another work...

 include/uapi/linux/if_packet.h | 13 ++++++++++---
 net/packet/af_packet.c         | 10 ++++++++--
 2 files changed, 18 insertions(+), 5 deletions(-)

diff --git a/include/uapi/linux/if_packet.h b/include/uapi/linux/if_packet.h
index 1e24aa7..7a8dbcf 100644
--- a/include/uapi/linux/if_packet.h
+++ b/include/uapi/linux/if_packet.h
@@ -84,7 +84,7 @@ struct tpacket_auxdata {
 	__u16		tp_mac;
 	__u16		tp_net;
 	__u16		tp_vlan_tci;
-	__u16		tp_padding;
+	__u16		tp_vlan_tpid;
 };
 
 /* Rx ring - header status */
@@ -93,7 +93,7 @@ struct tpacket_auxdata {
 #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_VLAN_VALID	(1 << 4) /* auxdata has valid tp_vlan_tci and tp_vlan_tpid */
 #define TP_STATUS_BLK_TMO	(1 << 5)
 
 /* Tx ring - header status */
@@ -133,7 +133,7 @@ struct tpacket2_hdr {
 	__u32		tp_sec;
 	__u32		tp_nsec;
 	__u16		tp_vlan_tci;
-	__u16		tp_padding;
+	__u16		tp_vlan_tpid;
 };
 
 struct tpacket_hdr_variant1 {
@@ -141,6 +141,12 @@ struct tpacket_hdr_variant1 {
 	__u32	tp_vlan_tci;
 };
 
+struct tpacket_hdr_variant2 {
+	__u32	tp_rxhash;
+	__u32	tp_vlan_tci;
+	__u32	tp_vlan_tpid;
+};
+
 struct tpacket3_hdr {
 	__u32		tp_next_offset;
 	__u32		tp_sec;
@@ -153,6 +159,7 @@ struct tpacket3_hdr {
 	/* pkt_hdr variants */
 	union {
 		struct tpacket_hdr_variant1 hv1;
+		struct tpacket_hdr_variant2 hv2;
 	};
 };
 
diff --git a/net/packet/af_packet.c b/net/packet/af_packet.c
index 9d70f13..3c75878 100644
--- a/net/packet/af_packet.c
+++ b/net/packet/af_packet.c
@@ -975,11 +975,15 @@ static void prb_clear_rxhash(struct tpacket_kbdq_core *pkc,
 static void prb_fill_vlan_info(struct tpacket_kbdq_core *pkc,
 			struct tpacket3_hdr *ppd)
 {
+	BUILD_BUG_ON(TPACKET_ALIGN(sizeof(*ppd)) != 48);
+
 	if (vlan_tx_tag_present(pkc->skb)) {
 		ppd->hv1.tp_vlan_tci = vlan_tx_tag_get(pkc->skb);
+		ppd->hv2.tp_vlan_tpid = (__force __u32)ntohs(pkc->skb->vlan_proto);
 		ppd->tp_status = TP_STATUS_VLAN_VALID;
 	} else {
 		ppd->hv1.tp_vlan_tci = 0;
+		ppd->hv2.tp_vlan_tpid = 0;
 		ppd->tp_status = TP_STATUS_AVAILABLE;
 	}
 }
@@ -1918,11 +1922,12 @@ 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);
+			h.h2->tp_vlan_tpid = ntohs(skb->vlan_proto);
 			status |= TP_STATUS_VLAN_VALID;
 		} else {
 			h.h2->tp_vlan_tci = 0;
+			h.h2->tp_vlan_tpid = 0;
 		}
-		h.h2->tp_padding = 0;
 		hdrlen = sizeof(*h.h2);
 		break;
 	case TPACKET_V3:
@@ -2867,11 +2872,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_vlan_tpid = ntohs(skb->vlan_proto);
 			aux.tp_status |= TP_STATUS_VLAN_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);
 	}
 
-- 
1.8.1.5