3 files changed, 57 insertions, 7 deletions

diff --git a/drivers/infiniband/hw/mlx4/main.c b/drivers/infiniband/hw/mlx4/main.c
index 3530c41fcd1f..8a3a2037b005 100644
--- a/drivers/infiniband/hw/mlx4/main.c
+++ b/drivers/infiniband/hw/mlx4/main.c
@@ -718,26 +718,53 @@ int mlx4_ib_add_mc(struct mlx4_ib_dev *mdev, struct mlx4_ib_qp *mqp,
         return ret;
 }
 
+struct mlx4_ib_steering {
+        struct list_head list;
+        u64 reg_id;
+        union ib_gid gid;
+};
+
 static int mlx4_ib_mcg_attach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
 {
         int err;
         struct mlx4_ib_dev *mdev = to_mdev(ibqp->device);
         struct mlx4_ib_qp *mqp = to_mqp(ibqp);
+        u64 reg_id;
+        struct mlx4_ib_steering *ib_steering = NULL;
+
+        if (mdev->dev->caps.steering_mode ==
+            MLX4_STEERING_MODE_DEVICE_MANAGED) {
+                ib_steering = kmalloc(sizeof(*ib_steering), GFP_KERNEL);
+                if (!ib_steering)
+                        return -ENOMEM;
+        }
 
-        err = mlx4_multicast_attach(mdev->dev, &mqp->mqp, gid->raw,
-                                    !!(mqp->flags & MLX4_IB_QP_BLOCK_MULTICAST_LOOPBACK),
-                                    MLX4_PROT_IB_IPV6);
+        err = mlx4_multicast_attach(mdev->dev, &mqp->mqp, gid->raw, mqp->port,
+                                    !!(mqp->flags &
+                                       MLX4_IB_QP_BLOCK_MULTICAST_LOOPBACK),
+                                    MLX4_PROT_IB_IPV6, &reg_id);
         if (err)
-                return err;
+                goto err_malloc;
 
         err = add_gid_entry(ibqp, gid);
         if (err)
                 goto err_add;
 
+        if (ib_steering) {
+                memcpy(ib_steering->gid.raw, gid->raw, 16);
+                ib_steering->reg_id = reg_id;
+                mutex_lock(&mqp->mutex);
+                list_add(&ib_steering->list, &mqp->steering_rules);
+                mutex_unlock(&mqp->mutex);
+        }
         return 0;
 
 err_add:
-        mlx4_multicast_detach(mdev->dev, &mqp->mqp, gid->raw, MLX4_PROT_IB_IPV6);
+        mlx4_multicast_detach(mdev->dev, &mqp->mqp, gid->raw,
+                              MLX4_PROT_IB_IPV6, reg_id);
+err_malloc:
+        kfree(ib_steering);
+
         return err;
 }
 
@@ -765,9 +792,30 @@ static int mlx4_ib_mcg_detach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
         u8 mac[6];
         struct net_device *ndev;
         struct mlx4_ib_gid_entry *ge;
+        u64 reg_id = 0;
+
+        if (mdev->dev->caps.steering_mode ==
+            MLX4_STEERING_MODE_DEVICE_MANAGED) {
+                struct mlx4_ib_steering *ib_steering;
+
+                mutex_lock(&mqp->mutex);
+                list_for_each_entry(ib_steering, &mqp->steering_rules, list) {
+                        if (!memcmp(ib_steering->gid.raw, gid->raw, 16)) {
+                                list_del(&ib_steering->list);
+                                break;
+                        }
+                }
+                mutex_unlock(&mqp->mutex);
+                if (&ib_steering->list == &mqp->steering_rules) {
+                        pr_err("Couldn't find reg_id for mgid. Steering rule is left attached\n");
+                        return -EINVAL;
+                }
+                reg_id = ib_steering->reg_id;
+                kfree(ib_steering);
+        }
 
-        err = mlx4_multicast_detach(mdev->dev,
-                                    &mqp->mqp, gid->raw, MLX4_PROT_IB_IPV6);
+        err = mlx4_multicast_detach(mdev->dev, &mqp->mqp, gid->raw,
+                                    MLX4_PROT_IB_IPV6, reg_id);
         if (err)
                 return err;
 
diff --git a/drivers/infiniband/hw/mlx4/mlx4_ib.h b/drivers/infiniband/hw/mlx4/mlx4_ib.h
index ff36655d23d3..42df4f7a6a5b 100644
--- a/drivers/infiniband/hw/mlx4/mlx4_ib.h
+++ b/drivers/infiniband/hw/mlx4/mlx4_ib.h
@@ -163,6 +163,7 @@ struct mlx4_ib_qp {
         u8                      state;
         int                     mlx_type;
         struct list_head        gid_list;
+        struct list_head        steering_rules;
 };
 
 struct mlx4_ib_srq {
diff --git a/drivers/infiniband/hw/mlx4/qp.c b/drivers/infiniband/hw/mlx4/qp.c
index 8d4ed24aef93..6af19f6c2b11 100644
--- a/drivers/infiniband/hw/mlx4/qp.c
+++ b/drivers/infiniband/hw/mlx4/qp.c
@@ -495,6 +495,7 @@ static int create_qp_common(struct mlx4_ib_dev *dev, struct ib_pd *pd,
         spin_lock_init(&qp->sq.lock);
         spin_lock_init(&qp->rq.lock);
         INIT_LIST_HEAD(&qp->gid_list);
+        INIT_LIST_HEAD(&qp->steering_rules);
 
         qp->state        = IB_QPS_RESET;
         if (init_attr->sq_sig_type == IB_SIGNAL_ALL_WR)