1 files changed, 105 insertions, 139 deletions

diff --git a/drivers/infiniband/core/cm.c b/drivers/infiniband/core/cm.c
index 07fb8d3c037f..8670e58675c6 100644
--- a/drivers/infiniband/core/cm.c
+++ b/drivers/infiniband/core/cm.c
@@ -35,6 +35,9 @@ MODULE_DESCRIPTION("InfiniBand CM");
 MODULE_LICENSE("Dual BSD/GPL");
 
 #define CM_DESTROY_ID_WAIT_TIMEOUT 10000 /* msecs */
+#define CM_DIRECT_RETRY_CTX ((void *) 1UL)
+#define CM_MRA_SETTING 24 /* 4.096us * 2^24 = ~68.7 seconds */
+
 static const char * const ibcm_rej_reason_strs[] = {
 	[IB_CM_REJ_NO_QP]			= "no QP",
 	[IB_CM_REJ_NO_EEC]			= "no EEC",
@@ -93,8 +96,7 @@ static void cm_process_work(struct cm_id_private *cm_id_priv,
 			    struct cm_work *work);
 static int cm_send_sidr_rep_locked(struct cm_id_private *cm_id_priv,
 				   struct ib_cm_sidr_rep_param *param);
-static int cm_send_dreq_locked(struct cm_id_private *cm_id_priv,
-			       const void *private_data, u8 private_data_len);
+static void cm_issue_dreq(struct cm_id_private *cm_id_priv);
 static int cm_send_drep_locked(struct cm_id_private *cm_id_priv,
 			       void *private_data, u8 private_data_len);
 static int cm_send_rej_locked(struct cm_id_private *cm_id_priv,
@@ -166,7 +168,7 @@ struct cm_port {
 struct cm_device {
 	struct kref kref;
 	struct list_head list;
-	spinlock_t mad_agent_lock;
+	rwlock_t mad_agent_lock;
 	struct ib_device *ib_device;
 	u8 ack_delay;
 	int going_down;
@@ -240,7 +242,6 @@ struct cm_id_private {
 	u8 initiator_depth;
 	u8 retry_count;
 	u8 rnr_retry_count;
-	u8 service_timeout;
 	u8 target_ack_delay;
 
 	struct list_head work_list;
@@ -284,7 +285,7 @@ static struct ib_mad_send_buf *cm_alloc_msg(struct cm_id_private *cm_id_priv)
 	if (!cm_id_priv->av.port)
 		return ERR_PTR(-EINVAL);
 
-	spin_lock(&cm_id_priv->av.port->cm_dev->mad_agent_lock);
+	read_lock(&cm_id_priv->av.port->cm_dev->mad_agent_lock);
 	mad_agent = cm_id_priv->av.port->mad_agent;
 	if (!mad_agent) {
 		m = ERR_PTR(-EINVAL);
@@ -307,30 +308,22 @@ static struct ib_mad_send_buf *cm_alloc_msg(struct cm_id_private *cm_id_priv)
 		goto out;
 	}
 
-	/* Timeout set by caller if response is expected. */
 	m->ah = ah;
-	m->retries = cm_id_priv->max_cm_retries;
-
-	refcount_inc(&cm_id_priv->refcount);
-	m->context[0] = cm_id_priv;
 
 out:
-	spin_unlock(&cm_id_priv->av.port->cm_dev->mad_agent_lock);
+	read_unlock(&cm_id_priv->av.port->cm_dev->mad_agent_lock);
 	return m;
 }
 
 static void cm_free_msg(struct ib_mad_send_buf *msg)
 {
-	struct cm_id_private *cm_id_priv = msg->context[0];
-
 	if (msg->ah)
 		rdma_destroy_ah(msg->ah, 0);
-	cm_deref_id(cm_id_priv);
 	ib_free_send_mad(msg);
 }
 
 static struct ib_mad_send_buf *
-cm_alloc_priv_msg(struct cm_id_private *cm_id_priv)
+cm_alloc_priv_msg(struct cm_id_private *cm_id_priv, enum ib_cm_state state)
 {
 	struct ib_mad_send_buf *msg;
 
@@ -339,7 +332,15 @@ cm_alloc_priv_msg(struct cm_id_private *cm_id_priv)
 	msg = cm_alloc_msg(cm_id_priv);
 	if (IS_ERR(msg))
 		return msg;
+
 	cm_id_priv->msg = msg;
+	refcount_inc(&cm_id_priv->refcount);
+	msg->context[0] = cm_id_priv;
+	msg->context[1] = (void *) (unsigned long) state;
+
+	msg->retries = cm_id_priv->max_cm_retries;
+	msg->timeout_ms = cm_id_priv->timeout_ms;
+
 	return msg;
 }
 
@@ -358,13 +359,20 @@ static void cm_free_priv_msg(struct ib_mad_send_buf *msg)
 	ib_free_send_mad(msg);
 }
 
-static struct ib_mad_send_buf *cm_alloc_response_msg_no_ah(struct cm_port *port,
-							   struct ib_mad_recv_wc *mad_recv_wc)
+static struct ib_mad_send_buf *
+cm_alloc_response_msg_no_ah(struct cm_port *port,
+			    struct ib_mad_recv_wc *mad_recv_wc,
+			    bool direct_retry)
 {
-	return ib_create_send_mad(port->mad_agent, 1, mad_recv_wc->wc->pkey_index,
-				  0, IB_MGMT_MAD_HDR, IB_MGMT_MAD_DATA,
-				  GFP_ATOMIC,
-				  IB_MGMT_BASE_VERSION);
+	struct ib_mad_send_buf *m;
+
+	m = ib_create_send_mad(port->mad_agent, 1, mad_recv_wc->wc->pkey_index,
+			       0, IB_MGMT_MAD_HDR, IB_MGMT_MAD_DATA,
+			       GFP_ATOMIC, IB_MGMT_BASE_VERSION);
+	if (!IS_ERR(m))
+		m->context[0] = direct_retry ? CM_DIRECT_RETRY_CTX : NULL;
+
+	return m;
 }
 
 static int cm_create_response_msg_ah(struct cm_port *port,
@@ -384,12 +392,13 @@ static int cm_create_response_msg_ah(struct cm_port *port,
 
 static int cm_alloc_response_msg(struct cm_port *port,
 				 struct ib_mad_recv_wc *mad_recv_wc,
+				 bool direct_retry,
 				 struct ib_mad_send_buf **msg)
 {
 	struct ib_mad_send_buf *m;
 	int ret;
 
-	m = cm_alloc_response_msg_no_ah(port, mad_recv_wc);
+	m = cm_alloc_response_msg_no_ah(port, mad_recv_wc, direct_retry);
 	if (IS_ERR(m))
 		return PTR_ERR(m);
 
@@ -403,13 +412,6 @@ static int cm_alloc_response_msg(struct cm_port *port,
 	return 0;
 }
 
-static void cm_free_response_msg(struct ib_mad_send_buf *msg)
-{
-	if (msg->ah)
-		rdma_destroy_ah(msg->ah, 0);
-	ib_free_send_mad(msg);
-}
-
 static void *cm_copy_private_data(const void *private_data, u8 private_data_len)
 {
 	void *data;
@@ -1109,7 +1111,8 @@ retest:
 			cm_id->state = IB_CM_IDLE;
 			break;
 		}
-		cm_send_dreq_locked(cm_id_priv, NULL, 0);
+		cm_issue_dreq(cm_id_priv);
+		cm_enter_timewait(cm_id_priv);
 		goto retest;
 	case IB_CM_DREQ_SENT:
 		ib_cancel_mad(cm_id_priv->msg);
@@ -1294,10 +1297,10 @@ static __be64 cm_form_tid(struct cm_id_private *cm_id_priv)
 	if (!cm_id_priv->av.port)
 		return cpu_to_be64(low_tid);
 
-	spin_lock(&cm_id_priv->av.port->cm_dev->mad_agent_lock);
+	read_lock(&cm_id_priv->av.port->cm_dev->mad_agent_lock);
 	if (cm_id_priv->av.port->mad_agent)
 		hi_tid = ((u64)cm_id_priv->av.port->mad_agent->hi_tid) << 32;
-	spin_unlock(&cm_id_priv->av.port->cm_dev->mad_agent_lock);
+	read_unlock(&cm_id_priv->av.port->cm_dev->mad_agent_lock);
 	return cpu_to_be64(hi_tid | low_tid);
 }
 
@@ -1557,7 +1560,7 @@ int ib_send_cm_req(struct ib_cm_id *cm_id,
 	if (param->alternate_path)
 		cm_move_av_from_path(&cm_id_priv->alt_av, &alt_av);
 
-	msg = cm_alloc_priv_msg(cm_id_priv);
+	msg = cm_alloc_priv_msg(cm_id_priv, IB_CM_REQ_SENT);
 	if (IS_ERR(msg)) {
 		ret = PTR_ERR(msg);
 		goto out_unlock;
@@ -1566,8 +1569,6 @@ int ib_send_cm_req(struct ib_cm_id *cm_id,
 	req_msg = (struct cm_req_msg *)msg->mad;
 	cm_format_req(req_msg, cm_id_priv, param);
 	cm_id_priv->tid = req_msg->hdr.tid;
-	msg->timeout_ms = cm_id_priv->timeout_ms;
-	msg->context[1] = (void *)(unsigned long)IB_CM_REQ_SENT;
 
 	cm_id_priv->local_qpn = cpu_to_be32(IBA_GET(CM_REQ_LOCAL_QPN, req_msg));
 	cm_id_priv->rq_psn = cpu_to_be32(IBA_GET(CM_REQ_STARTING_PSN, req_msg));
@@ -1598,7 +1599,7 @@ static int cm_issue_rej(struct cm_port *port,
 	struct cm_rej_msg *rej_msg, *rcv_msg;
 	int ret;
 
-	ret = cm_alloc_response_msg(port, mad_recv_wc, &msg);
+	ret = cm_alloc_response_msg(port, mad_recv_wc, false, &msg);
 	if (ret)
 		return ret;
 
@@ -1624,7 +1625,7 @@ static int cm_issue_rej(struct cm_port *port,
 		IBA_GET(CM_REJ_REMOTE_COMM_ID, rcv_msg));
 	ret = ib_post_send_mad(msg, NULL);
 	if (ret)
-		cm_free_response_msg(msg);
+		cm_free_msg(msg);
 
 	return ret;
 }
@@ -1871,7 +1872,7 @@ static void cm_process_work(struct cm_id_private *cm_id_priv,
 
 static void cm_format_mra(struct cm_mra_msg *mra_msg,
 			  struct cm_id_private *cm_id_priv,
-			  enum cm_msg_response msg_mraed, u8 service_timeout,
+			  enum cm_msg_response msg_mraed,
 			  const void *private_data, u8 private_data_len)
 {
 	cm_format_mad_hdr(&mra_msg->hdr, CM_MRA_ATTR_ID, cm_id_priv->tid);
@@ -1880,7 +1881,7 @@ static void cm_format_mra(struct cm_mra_msg *mra_msg,
 		be32_to_cpu(cm_id_priv->id.local_id));
 	IBA_SET(CM_MRA_REMOTE_COMM_ID, mra_msg,
 		be32_to_cpu(cm_id_priv->id.remote_id));
-	IBA_SET(CM_MRA_SERVICE_TIMEOUT, mra_msg, service_timeout);
+	IBA_SET(CM_MRA_SERVICE_TIMEOUT, mra_msg, CM_MRA_SETTING);
 
 	if (private_data && private_data_len)
 		IBA_SET_MEM(CM_MRA_PRIVATE_DATA, mra_msg, private_data,
@@ -1951,7 +1952,7 @@ static void cm_dup_req_handler(struct cm_work *work,
 	}
 	spin_unlock_irq(&cm_id_priv->lock);
 
-	ret = cm_alloc_response_msg(work->port, work->mad_recv_wc, &msg);
+	ret = cm_alloc_response_msg(work->port, work->mad_recv_wc, true, &msg);
 	if (ret)
 		return;
 
@@ -1959,7 +1960,7 @@ static void cm_dup_req_handler(struct cm_work *work,
 	switch (cm_id_priv->id.state) {
 	case IB_CM_MRA_REQ_SENT:
 		cm_format_mra((struct cm_mra_msg *) msg->mad, cm_id_priv,
-			      CM_MSG_RESPONSE_REQ, cm_id_priv->service_timeout,
+			      CM_MSG_RESPONSE_REQ,
 			      cm_id_priv->private_data,
 			      cm_id_priv->private_data_len);
 		break;
@@ -1980,7 +1981,7 @@ static void cm_dup_req_handler(struct cm_work *work,
 	return;
 
 unlock:	spin_unlock_irq(&cm_id_priv->lock);
-free:	cm_free_response_msg(msg);
+free:	cm_free_msg(msg);
 }
 
 static struct cm_id_private *cm_match_req(struct cm_work *work,
@@ -2294,7 +2295,7 @@ int ib_send_cm_rep(struct ib_cm_id *cm_id,
 		goto out;
 	}
 
-	msg = cm_alloc_priv_msg(cm_id_priv);
+	msg = cm_alloc_priv_msg(cm_id_priv, IB_CM_REP_SENT);
 	if (IS_ERR(msg)) {
 		ret = PTR_ERR(msg);
 		goto out;
@@ -2302,8 +2303,6 @@ int ib_send_cm_rep(struct ib_cm_id *cm_id,
 
 	rep_msg = (struct cm_rep_msg *) msg->mad;
 	cm_format_rep(rep_msg, cm_id_priv, param);
-	msg->timeout_ms = cm_id_priv->timeout_ms;
-	msg->context[1] = (void *) (unsigned long) IB_CM_REP_SENT;
 
 	trace_icm_send_rep(cm_id);
 	ret = ib_post_send_mad(msg, NULL);
@@ -2444,7 +2443,7 @@ static void cm_dup_rep_handler(struct cm_work *work)
 
 	atomic_long_inc(
 		&work->port->counters[CM_RECV_DUPLICATES][CM_REP_COUNTER]);
-	ret = cm_alloc_response_msg(work->port, work->mad_recv_wc, &msg);
+	ret = cm_alloc_response_msg(work->port, work->mad_recv_wc, true, &msg);
 	if (ret)
 		goto deref;
 
@@ -2455,7 +2454,7 @@ static void cm_dup_rep_handler(struct cm_work *work)
 			      cm_id_priv->private_data_len);
 	else if (cm_id_priv->id.state == IB_CM_MRA_REP_SENT)
 		cm_format_mra((struct cm_mra_msg *) msg->mad, cm_id_priv,
-			      CM_MSG_RESPONSE_REP, cm_id_priv->service_timeout,
+			      CM_MSG_RESPONSE_REP,
 			      cm_id_priv->private_data,
 			      cm_id_priv->private_data_len);
 	else
@@ -2469,7 +2468,7 @@ static void cm_dup_rep_handler(struct cm_work *work)
 	goto deref;
 
 unlock:	spin_unlock_irq(&cm_id_priv->lock);
-free:	cm_free_response_msg(msg);
+free:	cm_free_msg(msg);
 deref:	cm_deref_id(cm_id_priv);
 }
 
@@ -2653,59 +2652,68 @@ static void cm_format_dreq(struct cm_dreq_msg *dreq_msg,
 			    private_data_len);
 }
 
-static int cm_send_dreq_locked(struct cm_id_private *cm_id_priv,
-			       const void *private_data, u8 private_data_len)
+static void cm_issue_dreq(struct cm_id_private *cm_id_priv)
 {
 	struct ib_mad_send_buf *msg;
 	int ret;
 
 	lockdep_assert_held(&cm_id_priv->lock);
 
+	msg = cm_alloc_msg(cm_id_priv);
+	if (IS_ERR(msg))
+		return;
+
+	cm_format_dreq((struct cm_dreq_msg *) msg->mad, cm_id_priv, NULL, 0);
+
+	trace_icm_send_dreq(&cm_id_priv->id);
+	ret = ib_post_send_mad(msg, NULL);
+	if (ret)
+		cm_free_msg(msg);
+}
+
+int ib_send_cm_dreq(struct ib_cm_id *cm_id, const void *private_data,
+		    u8 private_data_len)
+{
+	struct cm_id_private *cm_id_priv =
+		container_of(cm_id, struct cm_id_private, id);
+	struct ib_mad_send_buf *msg;
+	unsigned long flags;
+	int ret;
+
 	if (private_data && private_data_len > IB_CM_DREQ_PRIVATE_DATA_SIZE)
 		return -EINVAL;
 
+	spin_lock_irqsave(&cm_id_priv->lock, flags);
 	if (cm_id_priv->id.state != IB_CM_ESTABLISHED) {
 		trace_icm_dreq_skipped(&cm_id_priv->id);
-		return -EINVAL;
+		ret = -EINVAL;
+		goto unlock;
 	}
 
 	if (cm_id_priv->id.lap_state == IB_CM_LAP_SENT ||
 	    cm_id_priv->id.lap_state == IB_CM_MRA_LAP_RCVD)
 		ib_cancel_mad(cm_id_priv->msg);
 
-	msg = cm_alloc_priv_msg(cm_id_priv);
+	msg = cm_alloc_priv_msg(cm_id_priv, IB_CM_DREQ_SENT);
 	if (IS_ERR(msg)) {
 		cm_enter_timewait(cm_id_priv);
-		return PTR_ERR(msg);
+		ret = PTR_ERR(msg);
+		goto unlock;
 	}
 
 	cm_format_dreq((struct cm_dreq_msg *) msg->mad, cm_id_priv,
 		       private_data, private_data_len);
-	msg->timeout_ms = cm_id_priv->timeout_ms;
-	msg->context[1] = (void *) (unsigned long) IB_CM_DREQ_SENT;
 
 	trace_icm_send_dreq(&cm_id_priv->id);
 	ret = ib_post_send_mad(msg, NULL);
 	if (ret) {
 		cm_enter_timewait(cm_id_priv);
 		cm_free_priv_msg(msg);
-		return ret;
+		goto unlock;
 	}
 
 	cm_id_priv->id.state = IB_CM_DREQ_SENT;
-	return 0;
-}
-
-int ib_send_cm_dreq(struct ib_cm_id *cm_id, const void *private_data,
-		    u8 private_data_len)
-{
-	struct cm_id_private *cm_id_priv =
-		container_of(cm_id, struct cm_id_private, id);
-	unsigned long flags;
-	int ret;
-
-	spin_lock_irqsave(&cm_id_priv->lock, flags);
-	ret = cm_send_dreq_locked(cm_id_priv, private_data, private_data_len);
+unlock:
 	spin_unlock_irqrestore(&cm_id_priv->lock, flags);
 	return ret;
 }
@@ -2791,7 +2799,7 @@ static int cm_issue_drep(struct cm_port *port,
 	struct cm_drep_msg *drep_msg;
 	int ret;
 
-	ret = cm_alloc_response_msg(port, mad_recv_wc, &msg);
+	ret = cm_alloc_response_msg(port, mad_recv_wc, true, &msg);
 	if (ret)
 		return ret;
 
@@ -2809,7 +2817,7 @@ static int cm_issue_drep(struct cm_port *port,
 		IBA_GET(CM_DREQ_REMOTE_COMM_ID, dreq_msg));
 	ret = ib_post_send_mad(msg, NULL);
 	if (ret)
-		cm_free_response_msg(msg);
+		cm_free_msg(msg);
 
 	return ret;
 }
@@ -2856,7 +2864,8 @@ static int cm_dreq_handler(struct cm_work *work)
 	case IB_CM_TIMEWAIT:
 		atomic_long_inc(&work->port->counters[CM_RECV_DUPLICATES]
 						     [CM_DREQ_COUNTER]);
-		msg = cm_alloc_response_msg_no_ah(work->port, work->mad_recv_wc);
+		msg = cm_alloc_response_msg_no_ah(work->port, work->mad_recv_wc,
+						  true);
 		if (IS_ERR(msg))
 			goto unlock;
 
@@ -2867,7 +2876,7 @@ static int cm_dreq_handler(struct cm_work *work)
 
 		if (cm_create_response_msg_ah(work->port, work->mad_recv_wc, msg) ||
 		    ib_post_send_mad(msg, NULL))
-			cm_free_response_msg(msg);
+			cm_free_msg(msg);
 		goto deref;
 	case IB_CM_DREQ_RCVD:
 		atomic_long_inc(&work->port->counters[CM_RECV_DUPLICATES]
@@ -3085,26 +3094,13 @@ out:
 	return -EINVAL;
 }
 
-int ib_send_cm_mra(struct ib_cm_id *cm_id,
-		   u8 service_timeout,
-		   const void *private_data,
-		   u8 private_data_len)
+int ib_prepare_cm_mra(struct ib_cm_id *cm_id)
 {
 	struct cm_id_private *cm_id_priv;
-	struct ib_mad_send_buf *msg;
 	enum ib_cm_state cm_state;
 	enum ib_cm_lap_state lap_state;
-	enum cm_msg_response msg_response;
-	void *data;
 	unsigned long flags;
-	int ret;
-
-	if (private_data && private_data_len > IB_CM_MRA_PRIVATE_DATA_SIZE)
-		return -EINVAL;
-
-	data = cm_copy_private_data(private_data, private_data_len);
-	if (IS_ERR(data))
-		return PTR_ERR(data);
+	int ret = 0;
 
 	cm_id_priv = container_of(cm_id, struct cm_id_private, id);
 
@@ -3113,58 +3109,33 @@ int ib_send_cm_mra(struct ib_cm_id *cm_id,
 	case IB_CM_REQ_RCVD:
 		cm_state = IB_CM_MRA_REQ_SENT;
 		lap_state = cm_id->lap_state;
-		msg_response = CM_MSG_RESPONSE_REQ;
 		break;
 	case IB_CM_REP_RCVD:
 		cm_state = IB_CM_MRA_REP_SENT;
 		lap_state = cm_id->lap_state;
-		msg_response = CM_MSG_RESPONSE_REP;
 		break;
 	case IB_CM_ESTABLISHED:
 		if (cm_id->lap_state == IB_CM_LAP_RCVD) {
 			cm_state = cm_id->state;
 			lap_state = IB_CM_MRA_LAP_SENT;
-			msg_response = CM_MSG_RESPONSE_OTHER;
 			break;
 		}
 		fallthrough;
 	default:
-		trace_icm_send_mra_unknown_err(&cm_id_priv->id);
+		trace_icm_prepare_mra_unknown_err(&cm_id_priv->id);
 		ret = -EINVAL;
 		goto error_unlock;
 	}
 
-	if (!(service_timeout & IB_CM_MRA_FLAG_DELAY)) {
-		msg = cm_alloc_msg(cm_id_priv);
-		if (IS_ERR(msg)) {
-			ret = PTR_ERR(msg);
-			goto error_unlock;
-		}
-
-		cm_format_mra((struct cm_mra_msg *) msg->mad, cm_id_priv,
-			      msg_response, service_timeout,
-			      private_data, private_data_len);
-		trace_icm_send_mra(cm_id);
-		ret = ib_post_send_mad(msg, NULL);
-		if (ret)
-			goto error_free_msg;
-	}
-
 	cm_id->state = cm_state;
 	cm_id->lap_state = lap_state;
-	cm_id_priv->service_timeout = service_timeout;
-	cm_set_private_data(cm_id_priv, data, private_data_len);
-	spin_unlock_irqrestore(&cm_id_priv->lock, flags);
-	return 0;
+	cm_set_private_data(cm_id_priv, NULL, 0);
 
-error_free_msg:
-	cm_free_msg(msg);
 error_unlock:
 	spin_unlock_irqrestore(&cm_id_priv->lock, flags);
-	kfree(data);
 	return ret;
 }
-EXPORT_SYMBOL(ib_send_cm_mra);
+EXPORT_SYMBOL(ib_prepare_cm_mra);
 
 static struct cm_id_private *cm_acquire_mraed_id(struct cm_mra_msg *mra_msg)
 {
@@ -3361,20 +3332,20 @@ static int cm_lap_handler(struct cm_work *work)
 	case IB_CM_MRA_LAP_SENT:
 		atomic_long_inc(&work->port->counters[CM_RECV_DUPLICATES]
 						     [CM_LAP_COUNTER]);
-		msg = cm_alloc_response_msg_no_ah(work->port, work->mad_recv_wc);
+		msg = cm_alloc_response_msg_no_ah(work->port, work->mad_recv_wc,
+						  true);
 		if (IS_ERR(msg))
 			goto unlock;
 
 		cm_format_mra((struct cm_mra_msg *) msg->mad, cm_id_priv,
 			      CM_MSG_RESPONSE_OTHER,
-			      cm_id_priv->service_timeout,
 			      cm_id_priv->private_data,
 			      cm_id_priv->private_data_len);
 		spin_unlock_irq(&cm_id_priv->lock);
 
 		if (cm_create_response_msg_ah(work->port, work->mad_recv_wc, msg) ||
 		    ib_post_send_mad(msg, NULL))
-			cm_free_response_msg(msg);
+			cm_free_msg(msg);
 		goto deref;
 	case IB_CM_LAP_RCVD:
 		atomic_long_inc(&work->port->counters[CM_RECV_DUPLICATES]
@@ -3513,7 +3484,7 @@ int ib_send_cm_sidr_req(struct ib_cm_id *cm_id,
 		goto out_unlock;
 	}
 
-	msg = cm_alloc_priv_msg(cm_id_priv);
+	msg = cm_alloc_priv_msg(cm_id_priv, IB_CM_SIDR_REQ_SENT);
 	if (IS_ERR(msg)) {
 		ret = PTR_ERR(msg);
 		goto out_unlock;
@@ -3521,8 +3492,6 @@ int ib_send_cm_sidr_req(struct ib_cm_id *cm_id,
 
 	cm_format_sidr_req((struct cm_sidr_req_msg *)msg->mad, cm_id_priv,
 			   param);
-	msg->timeout_ms = cm_id_priv->timeout_ms;
-	msg->context[1] = (void *)(unsigned long)IB_CM_SIDR_REQ_SENT;
 
 	trace_icm_send_sidr_req(&cm_id_priv->id);
 	ret = ib_post_send_mad(msg, NULL);
@@ -3768,17 +3737,18 @@ out:
 
 static void cm_process_send_error(struct cm_id_private *cm_id_priv,
 				  struct ib_mad_send_buf *msg,
-				  enum ib_cm_state state,
 				  enum ib_wc_status wc_status)
 {
+	enum ib_cm_state state = (unsigned long) msg->context[1];
 	struct ib_cm_event cm_event = {};
 	int ret;
 
-	/* Discard old sends or ones without a response. */
+	/* Discard old sends. */
 	spin_lock_irq(&cm_id_priv->lock);
 	if (msg != cm_id_priv->msg) {
 		spin_unlock_irq(&cm_id_priv->lock);
 		cm_free_msg(msg);
+		cm_deref_id(cm_id_priv);
 		return;
 	}
 	cm_free_priv_msg(msg);
@@ -3826,9 +3796,7 @@ static void cm_send_handler(struct ib_mad_agent *mad_agent,
 			    struct ib_mad_send_wc *mad_send_wc)
 {
 	struct ib_mad_send_buf *msg = mad_send_wc->send_buf;
-	struct cm_id_private *cm_id_priv = msg->context[0];
-	enum ib_cm_state state =
-		(enum ib_cm_state)(unsigned long)msg->context[1];
+	struct cm_id_private *cm_id_priv;
 	struct cm_port *port;
 	u16 attr_index;
 
@@ -3836,13 +3804,12 @@ static void cm_send_handler(struct ib_mad_agent *mad_agent,
 	attr_index = be16_to_cpu(((struct ib_mad_hdr *)
 				  msg->mad)->attr_id) - CM_ATTR_ID_OFFSET;
 
-	/*
-	 * If the send was in response to a received message (context[0] is not
-	 * set to a cm_id), and is not a REJ, then it is a send that was
-	 * manually retried.
-	 */
-	if (!cm_id_priv && (attr_index != CM_REJ_COUNTER))
+	if (msg->context[0] == CM_DIRECT_RETRY_CTX) {
 		msg->retries = 1;
+		cm_id_priv = NULL;
+	} else {
+		cm_id_priv = msg->context[0];
+	}
 
 	atomic_long_add(1 + msg->retries, &port->counters[CM_XMIT][attr_index]);
 	if (msg->retries)
@@ -3850,10 +3817,9 @@ static void cm_send_handler(struct ib_mad_agent *mad_agent,
 				&port->counters[CM_XMIT_RETRIES][attr_index]);
 
 	if (cm_id_priv)
-		cm_process_send_error(cm_id_priv, msg, state,
-				      mad_send_wc->status);
+		cm_process_send_error(cm_id_priv, msg, mad_send_wc->status);
 	else
-		cm_free_response_msg(msg);
+		cm_free_msg(msg);
 }
 
 static void cm_work_handler(struct work_struct *_work)
@@ -4374,7 +4340,7 @@ static int cm_add_one(struct ib_device *ib_device)
 		return -ENOMEM;
 
 	kref_init(&cm_dev->kref);
-	spin_lock_init(&cm_dev->mad_agent_lock);
+	rwlock_init(&cm_dev->mad_agent_lock);
 	cm_dev->ib_device = ib_device;
 	cm_dev->ack_delay = ib_device->attrs.local_ca_ack_delay;
 	cm_dev->going_down = 0;
@@ -4490,9 +4456,9 @@ static void cm_remove_one(struct ib_device *ib_device, void *client_data)
 		 * The above ensures no call paths from the work are running,
 		 * the remaining paths all take the mad_agent_lock.
 		 */
-		spin_lock(&cm_dev->mad_agent_lock);
+		write_lock(&cm_dev->mad_agent_lock);
 		port->mad_agent = NULL;
-		spin_unlock(&cm_dev->mad_agent_lock);
+		write_unlock(&cm_dev->mad_agent_lock);
 		ib_unregister_mad_agent(mad_agent);
 		ib_port_unregister_client_groups(ib_device, i,
 						 cm_counter_groups);