1 files changed, 134 insertions, 2 deletions

diff --git a/drivers/infiniband/core/cma.c b/drivers/infiniband/core/cma.c
index 9b471548e7ae..5b2d3ae3f9fc 100644
--- a/drivers/infiniband/core/cma.c
+++ b/drivers/infiniband/core/cma.c
@@ -2076,6 +2076,7 @@ static void _destroy_id(struct rdma_id_private *id_priv,
 	kfree(id_priv->id.route.path_rec);
 	kfree(id_priv->id.route.path_rec_inbound);
 	kfree(id_priv->id.route.path_rec_outbound);
+	kfree(id_priv->id.route.service_recs);
 
 	put_net(id_priv->id.route.addr.dev_addr.net);
 	kfree(id_priv);
@@ -3382,13 +3383,18 @@ err1:
 int rdma_resolve_route(struct rdma_cm_id *id, unsigned long timeout_ms)
 {
 	struct rdma_id_private *id_priv;
+	enum rdma_cm_state state;
 	int ret;
 
 	if (!timeout_ms)
 		return -EINVAL;
 
 	id_priv = container_of(id, struct rdma_id_private, id);
-	if (!cma_comp_exch(id_priv, RDMA_CM_ADDR_RESOLVED, RDMA_CM_ROUTE_QUERY))
+	state = id_priv->state;
+	if (!cma_comp_exch(id_priv, RDMA_CM_ADDR_RESOLVED,
+			   RDMA_CM_ROUTE_QUERY) &&
+	    !cma_comp_exch(id_priv, RDMA_CM_ADDRINFO_RESOLVED,
+			   RDMA_CM_ROUTE_QUERY))
 		return -EINVAL;
 
 	cma_id_get(id_priv);
@@ -3409,7 +3415,7 @@ int rdma_resolve_route(struct rdma_cm_id *id, unsigned long timeout_ms)
 
 	return 0;
 err:
-	cma_comp_exch(id_priv, RDMA_CM_ROUTE_QUERY, RDMA_CM_ADDR_RESOLVED);
+	cma_comp_exch(id_priv, RDMA_CM_ROUTE_QUERY, state);
 	cma_id_put(id_priv);
 	return ret;
 }
@@ -5506,3 +5512,129 @@ static void __exit cma_cleanup(void)
 
 module_init(cma_init);
 module_exit(cma_cleanup);
+
+static void cma_query_ib_service_handler(int status,
+					 struct sa_service_rec *recs,
+					 unsigned int num_recs, void *context)
+{
+	struct cma_work *work = context;
+	struct rdma_id_private *id_priv = work->id;
+	struct sockaddr_ib *addr;
+
+	if (status)
+		goto fail;
+
+	if (!num_recs) {
+		status = -ENOENT;
+		goto fail;
+	}
+
+	if (id_priv->id.route.service_recs) {
+		status = -EALREADY;
+		goto fail;
+	}
+
+	id_priv->id.route.service_recs =
+		kmalloc_array(num_recs, sizeof(*recs), GFP_KERNEL);
+	if (!id_priv->id.route.service_recs) {
+		status = -ENOMEM;
+		goto fail;
+	}
+
+	id_priv->id.route.num_service_recs = num_recs;
+	memcpy(id_priv->id.route.service_recs, recs, sizeof(*recs) * num_recs);
+
+	addr = (struct sockaddr_ib *)&id_priv->id.route.addr.dst_addr;
+	addr->sib_family = AF_IB;
+	addr->sib_addr = *(struct ib_addr *)&recs->gid;
+	addr->sib_pkey = recs->pkey;
+	addr->sib_sid = recs->id;
+	rdma_addr_set_dgid(&id_priv->id.route.addr.dev_addr,
+			   (union ib_gid *)&addr->sib_addr);
+	ib_addr_set_pkey(&id_priv->id.route.addr.dev_addr,
+			 ntohs(addr->sib_pkey));
+
+	queue_work(cma_wq, &work->work);
+	return;
+
+fail:
+	work->old_state = RDMA_CM_ADDRINFO_QUERY;
+	work->new_state = RDMA_CM_ADDR_BOUND;
+	work->event.event = RDMA_CM_EVENT_ADDRINFO_ERROR;
+	work->event.status = status;
+	pr_debug_ratelimited(
+		"RDMA CM: SERVICE_ERROR: failed to query service record. status %d\n",
+		status);
+	queue_work(cma_wq, &work->work);
+}
+
+static int cma_resolve_ib_service(struct rdma_id_private *id_priv,
+				  struct rdma_ucm_ib_service *ibs)
+{
+	struct sa_service_rec sr = {};
+	ib_sa_comp_mask mask = 0;
+	struct cma_work *work;
+
+	work = kzalloc(sizeof(*work), GFP_KERNEL);
+	if (!work)
+		return -ENOMEM;
+
+	cma_id_get(id_priv);
+
+	work->id = id_priv;
+	INIT_WORK(&work->work, cma_work_handler);
+	work->old_state = RDMA_CM_ADDRINFO_QUERY;
+	work->new_state = RDMA_CM_ADDRINFO_RESOLVED;
+	work->event.event = RDMA_CM_EVENT_ADDRINFO_RESOLVED;
+
+	if (ibs->flags & RDMA_USER_CM_IB_SERVICE_FLAG_ID) {
+		sr.id = cpu_to_be64(ibs->service_id);
+		mask |= IB_SA_SERVICE_REC_SERVICE_ID;
+	}
+	if (ibs->flags & RDMA_USER_CM_IB_SERVICE_FLAG_NAME) {
+		strscpy(sr.name, ibs->service_name, sizeof(sr.name));
+		mask |= IB_SA_SERVICE_REC_SERVICE_NAME;
+	}
+
+	id_priv->query_id = ib_sa_service_rec_get(&sa_client,
+						  id_priv->id.device,
+						  id_priv->id.port_num,
+						  &sr, mask,
+						  2000, GFP_KERNEL,
+						  cma_query_ib_service_handler,
+						  work, &id_priv->query);
+
+	if (id_priv->query_id < 0) {
+		cma_id_put(id_priv);
+		kfree(work);
+		return id_priv->query_id;
+	}
+
+	return 0;
+}
+
+int rdma_resolve_ib_service(struct rdma_cm_id *id,
+			    struct rdma_ucm_ib_service *ibs)
+{
+	struct rdma_id_private *id_priv;
+	int ret;
+
+	id_priv = container_of(id, struct rdma_id_private, id);
+	if (!id_priv->cma_dev ||
+	    !cma_comp_exch(id_priv, RDMA_CM_ADDR_BOUND, RDMA_CM_ADDRINFO_QUERY))
+		return -EINVAL;
+
+	if (rdma_cap_ib_sa(id->device, id->port_num))
+		ret = cma_resolve_ib_service(id_priv, ibs);
+	else
+		ret = -EOPNOTSUPP;
+
+	if (ret)
+		goto err;
+
+	return 0;
+err:
+	cma_comp_exch(id_priv, RDMA_CM_ADDRINFO_QUERY, RDMA_CM_ADDR_BOUND);
+	return ret;
+}
+EXPORT_SYMBOL(rdma_resolve_ib_service);