3 files changed, 44 insertions, 26 deletions

diff --git a/drivers/net/qlcnic/qlcnic.h b/drivers/net/qlcnic/qlcnic.h
index cc8385a6727e..c8caec90b31b 100644
--- a/drivers/net/qlcnic/qlcnic.h
+++ b/drivers/net/qlcnic/qlcnic.h
@@ -1013,6 +1013,7 @@ struct qlcnic_adapter {
 
 	u64 dev_rst_time;
 
+	struct vlan_group *vlgrp;
 	struct qlcnic_npar_info *npars;
 	struct qlcnic_eswitch *eswitch;
 	struct qlcnic_nic_template *nic_ops;
diff --git a/drivers/net/qlcnic/qlcnic_init.c b/drivers/net/qlcnic/qlcnic_init.c
index 26a7d6bca5c7..10cebb15ccd4 100644
--- a/drivers/net/qlcnic/qlcnic_init.c
+++ b/drivers/net/qlcnic/qlcnic_init.c
@@ -1380,24 +1380,28 @@ static struct sk_buff *qlcnic_process_rxbuf(struct qlcnic_adapter *adapter,
 }
 
 static int
-qlcnic_check_rx_tagging(struct qlcnic_adapter *adapter, struct sk_buff *skb)
+qlcnic_check_rx_tagging(struct qlcnic_adapter *adapter, struct sk_buff *skb,
+			u16 *vlan_tag)
 {
-	u16 vlan_tag;
 	struct ethhdr *eth_hdr;
 
-	if (!__vlan_get_tag(skb, &vlan_tag)) {
-		if (vlan_tag == adapter->pvid) {
-			/* strip the tag from the packet and send it up */
-			eth_hdr = (struct ethhdr *) skb->data;
-			memmove(skb->data + VLAN_HLEN, eth_hdr, ETH_ALEN * 2);
-			skb_pull(skb, VLAN_HLEN);
-			return 0;
-		}
+	if (!__vlan_get_tag(skb, vlan_tag)) {
+		eth_hdr = (struct ethhdr *) skb->data;
+		memmove(skb->data + VLAN_HLEN, eth_hdr, ETH_ALEN * 2);
+		skb_pull(skb, VLAN_HLEN);
+	}
+	if (!adapter->pvid)
+		return 0;
+
+	if (*vlan_tag == adapter->pvid) {
+		/* Outer vlan tag. Packet should follow non-vlan path */
+		*vlan_tag = 0xffff;
+		return 0;
 	}
 	if (adapter->flags & QLCNIC_TAGGING_ENABLED)
 		return 0;
 
-	return -EIO;
+	return -EINVAL;
 }
 
 static struct qlcnic_rx_buffer *
@@ -1411,6 +1415,7 @@ qlcnic_process_rcv(struct qlcnic_adapter *adapter,
 	struct sk_buff *skb;
 	struct qlcnic_host_rds_ring *rds_ring;
 	int index, length, cksum, pkt_offset;
+	u16 vid = 0xffff;
 
 	if (unlikely(ring >= adapter->max_rds_rings))
 		return NULL;
@@ -1441,17 +1446,18 @@ qlcnic_process_rcv(struct qlcnic_adapter *adapter,
 
 	skb->truesize = skb->len + sizeof(struct sk_buff);
 
-	if (unlikely(adapter->pvid)) {
-		if (qlcnic_check_rx_tagging(adapter, skb)) {
-			adapter->stats.rxdropped++;
-			dev_kfree_skb_any(skb);
-			return buffer;
-		}
+	if (unlikely(qlcnic_check_rx_tagging(adapter, skb, &vid))) {
+		adapter->stats.rxdropped++;
+		dev_kfree_skb(skb);
+		return buffer;
 	}
 
 	skb->protocol = eth_type_trans(skb, netdev);
 
-	napi_gro_receive(&sds_ring->napi, skb);
+	if ((vid != 0xffff) && adapter->vlgrp)
+		vlan_hwaccel_receive_skb(skb, adapter->vlgrp, vid);
+	else
+		napi_gro_receive(&sds_ring->napi, skb);
 
 	adapter->stats.rx_pkts++;
 	adapter->stats.rxbytes += length;
@@ -1480,6 +1486,7 @@ qlcnic_process_lro(struct qlcnic_adapter *adapter,
 	int index;
 	u16 lro_length, length, data_offset;
 	u32 seq_number;
+	u16 vid = 0xffff;
 
 	if (unlikely(ring > adapter->max_rds_rings))
 		return NULL;
@@ -1514,13 +1521,12 @@ qlcnic_process_lro(struct qlcnic_adapter *adapter,
 
 	skb_pull(skb, l2_hdr_offset);
 
-	if (unlikely(adapter->pvid)) {
-		if (qlcnic_check_rx_tagging(adapter, skb)) {
-			adapter->stats.rxdropped++;
-			dev_kfree_skb_any(skb);
-			return buffer;
-		}
+	if (unlikely(qlcnic_check_rx_tagging(adapter, skb, &vid))) {
+		adapter->stats.rxdropped++;
+		dev_kfree_skb(skb);
+		return buffer;
 	}
+
 	skb->protocol = eth_type_trans(skb, netdev);
 
 	iph = (struct iphdr *)skb->data;
@@ -1535,7 +1541,10 @@ qlcnic_process_lro(struct qlcnic_adapter *adapter,
 
 	length = skb->len;
 
-	netif_receive_skb(skb);
+	if ((vid != 0xffff) && adapter->vlgrp)
+		vlan_hwaccel_receive_skb(skb, adapter->vlgrp, vid);
+	else
+		netif_receive_skb(skb);
 
 	adapter->stats.lro_pkts++;
 	adapter->stats.lrobytes += length;
diff --git a/drivers/net/qlcnic/qlcnic_main.c b/drivers/net/qlcnic/qlcnic_main.c
index 5fd2abd1eb67..9eb0ced1ffab 100644
--- a/drivers/net/qlcnic/qlcnic_main.c
+++ b/drivers/net/qlcnic/qlcnic_main.c
@@ -371,6 +371,13 @@ static int qlcnic_set_mac(struct net_device *netdev, void *p)
 	return 0;
 }
 
+static void qlcnic_vlan_rx_register(struct net_device *netdev,
+		struct vlan_group *grp)
+{
+	struct qlcnic_adapter *adapter = netdev_priv(netdev);
+	adapter->vlgrp = grp;
+}
+
 static const struct net_device_ops qlcnic_netdev_ops = {
 	.ndo_open	   = qlcnic_open,
 	.ndo_stop	   = qlcnic_close,
@@ -381,6 +388,7 @@ static const struct net_device_ops qlcnic_netdev_ops = {
 	.ndo_set_mac_address    = qlcnic_set_mac,
 	.ndo_change_mtu	   = qlcnic_change_mtu,
 	.ndo_tx_timeout	   = qlcnic_tx_timeout,
+	.ndo_vlan_rx_register = qlcnic_vlan_rx_register,
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller = qlcnic_poll_controller,
 #endif
@@ -1446,7 +1454,7 @@ qlcnic_setup_netdev(struct qlcnic_adapter *adapter,
 	SET_ETHTOOL_OPS(netdev, &qlcnic_ethtool_ops);
 
 	netdev->features |= (NETIF_F_SG | NETIF_F_IP_CSUM |
-		NETIF_F_IPV6_CSUM | NETIF_F_GRO);
+		NETIF_F_IPV6_CSUM | NETIF_F_GRO | NETIF_F_HW_VLAN_RX);
 	netdev->vlan_features |= (NETIF_F_SG | NETIF_F_IP_CSUM |
 		NETIF_F_IPV6_CSUM);