3 files changed, 30 insertions, 0 deletions

diff --git a/drivers/net/mlx4/en_netdev.c b/drivers/net/mlx4/en_netdev.c
index a0d8a26f5a02..9a87c4f3bbbd 100644
--- a/drivers/net/mlx4/en_netdev.c
+++ b/drivers/net/mlx4/en_netdev.c
@@ -69,6 +69,7 @@ static void mlx4_en_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
         struct mlx4_en_priv *priv = netdev_priv(dev);
         struct mlx4_en_dev *mdev = priv->mdev;
         int err;
+        int idx;
 
         if (!priv->vlgrp)
                 return;
@@ -83,7 +84,10 @@ static void mlx4_en_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
                 if (err)
                         en_err(priv, "Failed configuring VLAN filter\n");
         }
+        if (mlx4_register_vlan(mdev->dev, priv->port, vid, &idx))
+                en_err(priv, "failed adding vlan %d\n", vid);
         mutex_unlock(&mdev->state_lock);
+
 }
 
 static void mlx4_en_vlan_rx_kill_vid(struct net_device *dev, unsigned short vid)
@@ -91,6 +95,7 @@ static void mlx4_en_vlan_rx_kill_vid(struct net_device *dev, unsigned short vid)
         struct mlx4_en_priv *priv = netdev_priv(dev);
         struct mlx4_en_dev *mdev = priv->mdev;
         int err;
+        int idx;
 
         if (!priv->vlgrp)
                 return;
@@ -101,6 +106,11 @@ static void mlx4_en_vlan_rx_kill_vid(struct net_device *dev, unsigned short vid)
 
         /* Remove VID from port VLAN filter */
         mutex_lock(&mdev->state_lock);
+        if (!mlx4_find_cached_vlan(mdev->dev, priv->port, vid, &idx))
+                mlx4_unregister_vlan(mdev->dev, priv->port, idx);
+        else
+                en_err(priv, "could not find vid %d in cache\n", vid);
+
         if (mdev->device_up && priv->port_up) {
                 err = mlx4_SET_VLAN_FLTR(mdev->dev, priv->port, priv->vlgrp);
                 if (err)
diff --git a/drivers/net/mlx4/mlx4_en.h b/drivers/net/mlx4/mlx4_en.h
index 449210994ee9..dab5eafb8946 100644
--- a/drivers/net/mlx4/mlx4_en.h
+++ b/drivers/net/mlx4/mlx4_en.h
@@ -463,6 +463,7 @@ struct mlx4_en_priv {
         char *mc_addrs;
         int mc_addrs_cnt;
         struct mlx4_en_stat_out_mbox hw_stats;
+        int vids[128];
 };
 
 
diff --git a/drivers/net/mlx4/port.c b/drivers/net/mlx4/port.c
index 606aa58afdea..56371ef328ef 100644
--- a/drivers/net/mlx4/port.c
+++ b/drivers/net/mlx4/port.c
@@ -182,6 +182,25 @@ static int mlx4_set_port_vlan_table(struct mlx4_dev *dev, u8 port,
         return err;
 }
 
+int mlx4_find_cached_vlan(struct mlx4_dev *dev, u8 port, u16 vid, int *idx)
+{
+        struct mlx4_vlan_table *table = &mlx4_priv(dev)->port[port].vlan_table;
+        int i;
+
+        for (i = 0; i < MLX4_MAX_VLAN_NUM; ++i) {
+                if (table->refs[i] &&
+                    (vid == (MLX4_VLAN_MASK &
+                              be32_to_cpu(table->entries[i])))) {
+                        /* VLAN already registered, increase reference count */
+                        *idx = i;
+                        return 0;
+                }
+        }
+
+        return -ENOENT;
+}
+EXPORT_SYMBOL_GPL(mlx4_find_cached_vlan);
+
 int mlx4_register_vlan(struct mlx4_dev *dev, u8 port, u16 vlan, int *index)
 {
         struct mlx4_vlan_table *table = &mlx4_priv(dev)->port[port].vlan_table;