2 files changed, 130 insertions, 2 deletions

diff --git a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
index 8eb24ee598e2..2c19dabccb9a 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
@@ -1016,6 +1016,126 @@ static void mlx4_en_do_multicast(struct mlx4_en_priv *priv,
         }
 }
 
+static void mlx4_en_do_uc_filter(struct mlx4_en_priv *priv,
+                                 struct net_device *dev,
+                                 struct mlx4_en_dev *mdev)
+{
+        struct netdev_hw_addr *ha;
+        struct mlx4_mac_entry *entry;
+        struct hlist_node *n, *tmp;
+        bool found;
+        u64 mac;
+        int err = 0;
+        struct hlist_head *bucket;
+        unsigned int i;
+        int removed = 0;
+        u32 prev_flags;
+
+        /* Note that we do not need to protect our mac_hash traversal with rcu,
+         * since all modification code is protected by mdev->state_lock
+         */
+
+        /* find what to remove */
+        for (i = 0; i < MLX4_EN_MAC_HASH_SIZE; ++i) {
+                bucket = &priv->mac_hash[i];
+                hlist_for_each_entry_safe(entry, n, tmp, bucket, hlist) {
+                        found = false;
+                        netdev_for_each_uc_addr(ha, dev) {
+                                if (ether_addr_equal_64bits(entry->mac,
+                                                            ha->addr)) {
+                                        found = true;
+                                        break;
+                                }
+                        }
+
+                        /* MAC address of the port is not in uc list */
+                        if (ether_addr_equal_64bits(entry->mac, dev->dev_addr))
+                                found = true;
+
+                        if (!found) {
+                                mac = mlx4_en_mac_to_u64(entry->mac);
+                                mlx4_en_uc_steer_release(priv, entry->mac,
+                                                         priv->base_qpn,
+                                                         entry->reg_id);
+                                mlx4_unregister_mac(mdev->dev, priv->port, mac);
+
+                                hlist_del_rcu(&entry->hlist);
+                                kfree_rcu(entry, rcu);
+                                en_dbg(DRV, priv, "Removed MAC %pM on port:%d\n",
+                                       entry->mac, priv->port);
+                                ++removed;
+                        }
+                }
+        }
+
+        /* if we didn't remove anything, there is no use in trying to add
+         * again once we are in a forced promisc mode state
+         */
+        if ((priv->flags & MLX4_EN_FLAG_FORCE_PROMISC) && 0 == removed)
+                return;
+
+        prev_flags = priv->flags;
+        priv->flags &= ~MLX4_EN_FLAG_FORCE_PROMISC;
+
+        /* find what to add */
+        netdev_for_each_uc_addr(ha, dev) {
+                found = false;
+                bucket = &priv->mac_hash[ha->addr[MLX4_EN_MAC_HASH_IDX]];
+                hlist_for_each_entry(entry, n, bucket, hlist) {
+                        if (ether_addr_equal_64bits(entry->mac, ha->addr)) {
+                                found = true;
+                                break;
+                        }
+                }
+
+                if (!found) {
+                        entry = kmalloc(sizeof(*entry), GFP_KERNEL);
+                        if (!entry) {
+                                en_err(priv, "Failed adding MAC %pM on port:%d (out of memory)\n",
+                                       ha->addr, priv->port);
+                                priv->flags |= MLX4_EN_FLAG_FORCE_PROMISC;
+                                break;
+                        }
+                        mac = mlx4_en_mac_to_u64(ha->addr);
+                        memcpy(entry->mac, ha->addr, ETH_ALEN);
+                        err = mlx4_register_mac(mdev->dev, priv->port, mac);
+                        if (err < 0) {
+                                en_err(priv, "Failed registering MAC %pM on port %d: %d\n",
+                                       ha->addr, priv->port, err);
+                                kfree(entry);
+                                priv->flags |= MLX4_EN_FLAG_FORCE_PROMISC;
+                                break;
+                        }
+                        err = mlx4_en_uc_steer_add(priv, ha->addr,
+                                                   &priv->base_qpn,
+                                                   &entry->reg_id);
+                        if (err) {
+                                en_err(priv, "Failed adding MAC %pM on port %d: %d\n",
+                                       ha->addr, priv->port, err);
+                                mlx4_unregister_mac(mdev->dev, priv->port, mac);
+                                kfree(entry);
+                                priv->flags |= MLX4_EN_FLAG_FORCE_PROMISC;
+                                break;
+                        } else {
+                                unsigned int mac_hash;
+                                en_dbg(DRV, priv, "Added MAC %pM on port:%d\n",
+                                       ha->addr, priv->port);
+                                mac_hash = ha->addr[MLX4_EN_MAC_HASH_IDX];
+                                bucket = &priv->mac_hash[mac_hash];
+                                hlist_add_head_rcu(&entry->hlist, bucket);
+                        }
+                }
+        }
+
+        if (priv->flags & MLX4_EN_FLAG_FORCE_PROMISC) {
+                en_warn(priv, "Forcing promiscuous mode on port:%d\n",
+                        priv->port);
+        } else if (prev_flags & MLX4_EN_FLAG_FORCE_PROMISC) {
+                en_warn(priv, "Stop forcing promiscuous mode on port:%d\n",
+                        priv->port);
+        }
+}
+
 static void mlx4_en_do_set_rx_mode(struct work_struct *work)
 {
         struct mlx4_en_priv *priv = container_of(work, struct mlx4_en_priv,
@@ -1043,8 +1163,12 @@ static void mlx4_en_do_set_rx_mode(struct work_struct *work)
                 }
         }
 
+        if (dev->priv_flags & IFF_UNICAST_FLT)
+                mlx4_en_do_uc_filter(priv, dev, mdev);
+
         /* Promsicuous mode: disable all filters */
-        if (dev->flags & IFF_PROMISC) {
+        if ((dev->flags & IFF_PROMISC) ||
+            (priv->flags & MLX4_EN_FLAG_FORCE_PROMISC)) {
                 mlx4_en_set_promisc_mode(priv, mdev);
                 goto out;
         }
@@ -1961,6 +2085,9 @@ int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
             MLX4_STEERING_MODE_DEVICE_MANAGED)
                 dev->hw_features |= NETIF_F_NTUPLE;
 
+        if (mdev->dev->caps.steering_mode != MLX4_STEERING_MODE_A0)
+                dev->priv_flags |= IFF_UNICAST_FLT;
+
         mdev->pndev[port] = dev;
 
         netif_carrier_off(dev);
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
index 0ff99e0b2ea3..c313d7e943a9 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
@@ -439,7 +439,8 @@ enum {
          */
         MLX4_EN_FLAG_ENABLE_HW_LOOPBACK = (1 << 2),
         /* whether we need to drop packets that hardware loopback-ed */
-        MLX4_EN_FLAG_RX_FILTER_NEEDED   = (1 << 3)
+        MLX4_EN_FLAG_RX_FILTER_NEEDED   = (1 << 3),
+        MLX4_EN_FLAG_FORCE_PROMISC      = (1 << 4)
 };
 
 #define MLX4_EN_MAC_HASH_SIZE (1 << BITS_PER_BYTE)