[PATCH 1/2] tun/tap: add the feature of vlan rx extraction

[root]

Tap is a virtual net device that has no vlan rx untag feature.
So this virtual device can not send/receive vlan packets in
kernel 2.6.x. To make this device support vlan send/receive vlan
packets in kernel 2.6.x, a vlan rx extraction feature is simulated
in its driver.

Signed-off-by: Zhu Yanjun <Yanjun.Zhu@xxxxxxxxxxxxx>
---
 drivers/net/tun.c         |  118 ++++++++++++++++++++++++++++++++++++++++++++-
 include/linux/netdevice.h |    1 +
 net/core/dev.c            |   13 +++++
 3 files changed, 131 insertions(+), 1 deletion(-)

diff --git a/drivers/net/tun.c b/drivers/net/tun.c
index 894ad84..029e6cf 100644
--- a/drivers/net/tun.c
+++ b/drivers/net/tun.c
@@ -69,6 +69,8 @@
 #include <asm/system.h>
 #include <asm/uaccess.h>
 
+#include <linux/if_vlan.h>
+
 /* Uncomment to enable debugging */
 /* #define TUN_DEBUG 1 */
 
@@ -426,6 +428,8 @@ static const struct net_device_ops tun_netdev_ops = {
 	.ndo_change_mtu		= tun_net_change_mtu,
 };
 
+static void tap_vlan_rx_register(struct net_device *dev, struct vlan_group *grp);
+
 static const struct net_device_ops tap_netdev_ops = {
 	.ndo_uninit		= tun_net_uninit,
 	.ndo_open		= tun_net_open,
@@ -435,6 +439,7 @@ static const struct net_device_ops tap_netdev_ops = {
 	.ndo_set_multicast_list	= tun_net_mclist,
 	.ndo_set_mac_address	= eth_mac_addr,
 	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_vlan_rx_register   = tap_vlan_rx_register,
 };
 
 /* Initialize net device. */
@@ -464,6 +469,8 @@ static void tun_net_init(struct net_device *dev)
 
 		random_ether_addr(dev->dev_addr);
 
+		dev->features |= NETIF_F_HW_VLAN_RX;
+
 		dev->tx_queue_len = TUN_READQ_SIZE;  /* We prefer our own queue length */
 		break;
 	}
@@ -530,6 +537,105 @@ static inline struct sk_buff *tun_alloc_skb(struct tun_struct *tun,
 	return skb;
 }
 
+static struct sk_buff *vlan_reorder_header(struct sk_buff *skb)
+{
+	if (skb_cow(skb, skb_headroom(skb)) < 0)
+		return NULL;
+	memmove(skb->data - ETH_HLEN, skb->data - VLAN_ETH_HLEN, 2 * ETH_ALEN);
+	skb->mac_header += VLAN_HLEN;
+	return skb;
+}
+
+static void vlan_set_encap_proto(struct sk_buff *skb, struct vlan_hdr *vhdr)
+{
+	__be16 proto;
+	unsigned char *rawp;
+
+	/*
+ * 	 * Was a VLAN packet, grab the encapsulated protocol, which the layer
+ * 	 	 * three protocols care about.
+ * 	 	 	 */
+
+	proto = vhdr->h_vlan_encapsulated_proto;
+	if (ntohs(proto) >= 1536) {
+		skb->protocol = proto;
+		return;
+	}
+
+	rawp = skb->data;
+	if (*(unsigned short *) rawp == 0xFFFF)
+		/*
+ * 		 * This is a magic hack to spot IPX packets. Older Novell
+ * 		 		 * breaks the protocol design and runs IPX over 802.3 without
+ * 		 		 		 * an 802.2 LLC layer. We look for FFFF which isn't a used
+ * 		 		 		 		 * 802.2 SSAP/DSAP. This won't work for fault tolerant netware
+ * 		 		 		 		 		 * but does for the rest.
+ * 		 		 		 		 		 		 */
+		skb->protocol = htons(ETH_P_802_3);
+	else
+		/*
+ * 		 * Real 802.2 LLC
+ * 		 		 */
+		skb->protocol = htons(ETH_P_802_2);
+}
+
+static void skb_reset_mac_len(struct sk_buff *skb)
+{
+	skb->mac_len = skb->network_header - skb->mac_header;
+}
+
+static struct sk_buff *vlan_untag(struct sk_buff *skb)
+{
+	struct vlan_hdr *vhdr;
+	u16 vlan_tci;
+
+	if (unlikely(vlan_tx_tag_present(skb))) {
+		/* vlan_tci is already set-up so leave this for another time */
+		return skb;
+	}
+
+	skb = skb_share_check(skb, GFP_ATOMIC);
+	if (unlikely(!skb))
+		goto err_free;
+
+	if (unlikely(!pskb_may_pull(skb, VLAN_HLEN)))
+		goto err_free;
+
+	vhdr = (struct vlan_hdr *) skb->data;
+	vlan_tci = ntohs(vhdr->h_vlan_TCI);
+	__vlan_hwaccel_put_tag(skb, vlan_tci);
+
+	skb_pull_rcsum(skb, VLAN_HLEN);
+	vlan_set_encap_proto(skb, vhdr);
+
+	skb = vlan_reorder_header(skb);
+	if (unlikely(!skb))
+		goto err_free;
+
+	skb_reset_network_header(skb);
+	skb_reset_transport_header(skb);
+	skb_reset_mac_len(skb);
+
+	return skb;
+
+err_free:
+	kfree_skb(skb);
+	return NULL;
+}
+
+static struct vlan_group *g_vlgrp = NULL;
+static void tap_vlan_rx_register(struct net_device *dev,
+		struct vlan_group *grp)
+{
+	unsigned long flags;
+	local_irq_save(flags);
+
+	printk(KERN_DEBUG "zhuyj func:%s,line:%d\n", __FUNCTION__, __LINE__);
+	g_vlgrp = grp;
+	tun_net_change_mtu(dev, dev->mtu);
+	local_irq_restore(flags);
+}
+
 /* Get packet from user space buffer */
 static __inline__ ssize_t tun_get_user(struct tun_struct *tun,
 				       const struct iovec *iv, size_t count,
@@ -655,7 +761,17 @@ static __inline__ ssize_t tun_get_user(struct tun_struct *tun,
 		skb_shinfo(skb)->gso_segs = 0;
 	}
 
-	netif_rx_ni(skb);
+	if (g_vlgrp && (skb->protocol == cpu_to_be16(ETH_P_8021Q))){
+		struct vlan_hdr *vhdr;
+		u16 vlan_tci;
+		int ret;
+		vhdr = (struct vlan_hdr *) skb->data;
+		vlan_tci = ntohs(vhdr->h_vlan_TCI);
+		skb = vlan_untag(skb);
+		ret = vlan_netif_rx(skb, g_vlgrp, vlan_tci);
+	} else {
+		netif_rx_ni(skb);
+	}
 
 	tun->dev->stats.rx_packets++;
 	tun->dev->stats.rx_bytes += len;
diff --git a/include/linux/netdevice.h b/include/linux/netdevice.h
index 9d7e8f7..04c659b 100644
--- a/include/linux/netdevice.h
+++ b/include/linux/netdevice.h
@@ -1467,6 +1467,7 @@ extern void dev_kfree_skb_any(struct sk_buff *skb);
 #define HAVE_NETIF_RX 1
 extern int		netif_rx(struct sk_buff *skb);
 extern int		netif_rx_ni(struct sk_buff *skb);
+extern int		vlan_netif_rx(struct sk_buff *skb, struct vlan_group *grp, u16 vlan_tci);
 #define HAVE_NETIF_RECEIVE_SKB 1
 extern int		netif_receive_skb(struct sk_buff *skb);
 extern void		napi_gro_flush(struct napi_struct *napi);
diff --git a/net/core/dev.c b/net/core/dev.c
index d775563..a3802ca 100644
--- a/net/core/dev.c
+++ b/net/core/dev.c
@@ -2067,6 +2067,19 @@ int netif_rx_ni(struct sk_buff *skb)
 }
 EXPORT_SYMBOL(netif_rx_ni);
 
+int vlan_netif_rx(struct sk_buff *skb, struct vlan_group *grp, u16 vlan_tci)
+{
+	int ret;
+
+	preempt_disable();
+	ret = __vlan_hwaccel_rx(skb, grp, vlan_tci, 0);
+	if (local_softirq_pending())
+		do_softirq();
+	preempt_enable();
+	return ret;
+}
+EXPORT_SYMBOL(vlan_netif_rx);
+
 static void net_tx_action(struct softirq_action *h)
 {
 	struct softnet_data *sd = &__get_cpu_var(softnet_data);
-- 
1.7.9.5


--------------020500040601000703060900
Content-Type: text/x-patch;
 name="0002-vlan-Centralize-handling-of-hardware-acceleration.patch"
Content-Transfer-Encoding: 7bit
Content-Disposition: attachment;
 filename*0="0002-vlan-Centralize-handling-of-hardware-acceleration.patch"