1 files changed, 69 insertions, 20 deletions

diff --git a/drivers/net/qlcnic/qlcnic_main.c b/drivers/net/qlcnic/qlcnic_main.c
index 5d93b464681a..089212214ad4 100644
--- a/drivers/net/qlcnic/qlcnic_main.c
+++ b/drivers/net/qlcnic/qlcnic_main.c
@@ -759,6 +759,21 @@ qlcnic_check_options(struct qlcnic_adapter *adapter)
 }
 
 static void
+qlcnic_set_vlan_config(struct qlcnic_adapter *adapter,
+                struct qlcnic_esw_func_cfg *esw_cfg)
+{
+        if (esw_cfg->discard_tagged)
+                adapter->flags &= ~QLCNIC_TAGGING_ENABLED;
+        else
+                adapter->flags |= QLCNIC_TAGGING_ENABLED;
+
+        if (esw_cfg->vlan_id)
+                adapter->pvid = esw_cfg->vlan_id;
+        else
+                adapter->pvid = 0;
+}
+
+static void
 qlcnic_set_eswitch_port_features(struct qlcnic_adapter *adapter,
                 struct qlcnic_esw_func_cfg *esw_cfg)
 {
@@ -781,6 +796,7 @@ qlcnic_set_eswitch_port_config(struct qlcnic_adapter *adapter)
         esw_cfg.pci_func = adapter->ahw.pci_func;
         if (qlcnic_get_eswitch_port_config(adapter, &esw_cfg))
                         return -EIO;
+        qlcnic_set_vlan_config(adapter, &esw_cfg);
         qlcnic_set_eswitch_port_features(adapter, &esw_cfg);
 
         return 0;
@@ -1728,26 +1744,13 @@ qlcnic_tso_check(struct net_device *netdev,
 {
         u8 opcode = TX_ETHER_PKT;
         __be16 protocol = skb->protocol;
-        u16 flags = 0, vid = 0;
-        int copied, offset, copy_len, hdr_len = 0, tso = 0, vlan_oob = 0;
+        u16 flags = 0;
+        int copied, offset, copy_len, hdr_len = 0, tso = 0;
         struct cmd_desc_type0 *hwdesc;
         struct vlan_ethhdr *vh;
         struct qlcnic_adapter *adapter = netdev_priv(netdev);
         u32 producer = tx_ring->producer;
-
-        if (protocol == cpu_to_be16(ETH_P_8021Q)) {
-
-                vh = (struct vlan_ethhdr *)skb->data;
-                protocol = vh->h_vlan_encapsulated_proto;
-                flags = FLAGS_VLAN_TAGGED;
-
-        } else if (vlan_tx_tag_present(skb)) {
-
-                flags = FLAGS_VLAN_OOB;
-                vid = vlan_tx_tag_get(skb);
-                qlcnic_set_tx_vlan_tci(first_desc, vid);
-                vlan_oob = 1;
-        }
+        int vlan_oob = first_desc->flags_opcode & cpu_to_le16(FLAGS_VLAN_OOB);
 
         if (*(skb->data) & BIT_0) {
                 flags |= BIT_0;
@@ -1818,7 +1821,7 @@ qlcnic_tso_check(struct net_device *netdev,
                 vh = (struct vlan_ethhdr *)((char *)hwdesc + 2);
                 skb_copy_from_linear_data(skb, vh, 12);
                 vh->h_vlan_proto = htons(ETH_P_8021Q);
-                vh->h_vlan_TCI = htons(vid);
+                vh->h_vlan_TCI = htons(first_desc->vlan_TCI);
                 skb_copy_from_linear_data_offset(skb, 12,
                                 (char *)vh + 16, copy_len - 16);
 
@@ -1898,11 +1901,47 @@ out_err:
         return -ENOMEM;
 }
 
+static int
+qlcnic_check_tx_tagging(struct qlcnic_adapter *adapter,
+                        struct sk_buff *skb,
+                        struct cmd_desc_type0 *first_desc)
+{
+        u8 opcode = 0;
+        u16 flags = 0;
+        __be16 protocol = skb->protocol;
+        struct vlan_ethhdr *vh;
+
+        if (protocol == cpu_to_be16(ETH_P_8021Q)) {
+                vh = (struct vlan_ethhdr *)skb->data;
+                protocol = vh->h_vlan_encapsulated_proto;
+                flags = FLAGS_VLAN_TAGGED;
+                qlcnic_set_tx_vlan_tci(first_desc, ntohs(vh->h_vlan_TCI));
+        } else if (vlan_tx_tag_present(skb)) {
+                flags = FLAGS_VLAN_OOB;
+                qlcnic_set_tx_vlan_tci(first_desc, vlan_tx_tag_get(skb));
+        }
+        if (unlikely(adapter->pvid)) {
+                if (first_desc->vlan_TCI &&
+                                !(adapter->flags & QLCNIC_TAGGING_ENABLED))
+                        return -EIO;
+                if (first_desc->vlan_TCI &&
+                                (adapter->flags & QLCNIC_TAGGING_ENABLED))
+                        goto set_flags;
+
+                flags = FLAGS_VLAN_OOB;
+                qlcnic_set_tx_vlan_tci(first_desc, adapter->pvid);
+        }
+set_flags:
+        qlcnic_set_tx_flags_opcode(first_desc, flags, opcode);
+        return 0;
+}
+
 static inline void
 qlcnic_clear_cmddesc(u64 *desc)
 {
         desc[0] = 0ULL;
         desc[2] = 0ULL;
+        desc[7] = 0ULL;
 }
 
 netdev_tx_t
@@ -1952,6 +1991,12 @@ qlcnic_xmit_frame(struct sk_buff *skb, struct net_device *netdev)
 
         pdev = adapter->pdev;
 
+        first_desc = hwdesc = &tx_ring->desc_head[producer];
+        qlcnic_clear_cmddesc((u64 *)hwdesc);
+
+        if (qlcnic_check_tx_tagging(adapter, skb, first_desc))
+                goto drop_packet;
+
         if (qlcnic_map_tx_skb(pdev, skb, pbuf)) {
                 adapter->stats.tx_dma_map_error++;
                 goto drop_packet;
@@ -1960,9 +2005,6 @@ qlcnic_xmit_frame(struct sk_buff *skb, struct net_device *netdev)
         pbuf->skb = skb;
         pbuf->frag_count = frag_count;
 
-        first_desc = hwdesc = &tx_ring->desc_head[producer];
-        qlcnic_clear_cmddesc((u64 *)hwdesc);
-
         qlcnic_set_tx_frags_len(first_desc, frag_count, skb->len);
         qlcnic_set_tx_port(first_desc, adapter->portnum);
 
@@ -3349,6 +3391,13 @@ qlcnic_sysfs_write_esw_config(struct file *file, struct kobject *kobj,
                 case QLCNIC_PORT_DEFAULTS:
                         qlcnic_set_eswitch_port_features(adapter, &esw_cfg[i]);
                         break;
+                case QLCNIC_ADD_VLAN:
+                        qlcnic_set_vlan_config(adapter, &esw_cfg[i]);
+                        break;
+                case QLCNIC_DEL_VLAN:
+                        esw_cfg[i].vlan_id = 0;
+                        qlcnic_set_vlan_config(adapter, &esw_cfg[i]);
+                        break;
                 }
         }