xref: /openbmc/linux/drivers/infiniband/core/mad.c (revision b6dcefde)
1 /*
2  * Copyright (c) 2004-2007 Voltaire, Inc. All rights reserved.
3  * Copyright (c) 2005 Intel Corporation.  All rights reserved.
4  * Copyright (c) 2005 Mellanox Technologies Ltd.  All rights reserved.
5  * Copyright (c) 2009 HNR Consulting. All rights reserved.
6  *
7  * This software is available to you under a choice of one of two
8  * licenses.  You may choose to be licensed under the terms of the GNU
9  * General Public License (GPL) Version 2, available from the file
10  * COPYING in the main directory of this source tree, or the
11  * OpenIB.org BSD license below:
12  *
13  *     Redistribution and use in source and binary forms, with or
14  *     without modification, are permitted provided that the following
15  *     conditions are met:
16  *
17  *      - Redistributions of source code must retain the above
18  *        copyright notice, this list of conditions and the following
19  *        disclaimer.
20  *
21  *      - Redistributions in binary form must reproduce the above
22  *        copyright notice, this list of conditions and the following
23  *        disclaimer in the documentation and/or other materials
24  *        provided with the distribution.
25  *
26  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
27  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
28  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
29  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
30  * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
31  * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
32  * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
33  * SOFTWARE.
34  *
35  */
36 #include <linux/dma-mapping.h>
37 #include <rdma/ib_cache.h>
38 
39 #include "mad_priv.h"
40 #include "mad_rmpp.h"
41 #include "smi.h"
42 #include "agent.h"
43 
44 MODULE_LICENSE("Dual BSD/GPL");
45 MODULE_DESCRIPTION("kernel IB MAD API");
46 MODULE_AUTHOR("Hal Rosenstock");
47 MODULE_AUTHOR("Sean Hefty");
48 
49 int mad_sendq_size = IB_MAD_QP_SEND_SIZE;
50 int mad_recvq_size = IB_MAD_QP_RECV_SIZE;
51 
52 module_param_named(send_queue_size, mad_sendq_size, int, 0444);
53 MODULE_PARM_DESC(send_queue_size, "Size of send queue in number of work requests");
54 module_param_named(recv_queue_size, mad_recvq_size, int, 0444);
55 MODULE_PARM_DESC(recv_queue_size, "Size of receive queue in number of work requests");
56 
57 static struct kmem_cache *ib_mad_cache;
58 
59 static struct list_head ib_mad_port_list;
60 static u32 ib_mad_client_id = 0;
61 
62 /* Port list lock */
63 static DEFINE_SPINLOCK(ib_mad_port_list_lock);
64 
65 /* Forward declarations */
66 static int method_in_use(struct ib_mad_mgmt_method_table **method,
67 			 struct ib_mad_reg_req *mad_reg_req);
68 static void remove_mad_reg_req(struct ib_mad_agent_private *priv);
69 static struct ib_mad_agent_private *find_mad_agent(
70 					struct ib_mad_port_private *port_priv,
71 					struct ib_mad *mad);
72 static int ib_mad_post_receive_mads(struct ib_mad_qp_info *qp_info,
73 				    struct ib_mad_private *mad);
74 static void cancel_mads(struct ib_mad_agent_private *mad_agent_priv);
75 static void timeout_sends(struct work_struct *work);
76 static void local_completions(struct work_struct *work);
77 static int add_nonoui_reg_req(struct ib_mad_reg_req *mad_reg_req,
78 			      struct ib_mad_agent_private *agent_priv,
79 			      u8 mgmt_class);
80 static int add_oui_reg_req(struct ib_mad_reg_req *mad_reg_req,
81 			   struct ib_mad_agent_private *agent_priv);
82 
83 /*
84  * Returns a ib_mad_port_private structure or NULL for a device/port
85  * Assumes ib_mad_port_list_lock is being held
86  */
87 static inline struct ib_mad_port_private *
88 __ib_get_mad_port(struct ib_device *device, int port_num)
89 {
90 	struct ib_mad_port_private *entry;
91 
92 	list_for_each_entry(entry, &ib_mad_port_list, port_list) {
93 		if (entry->device == device && entry->port_num == port_num)
94 			return entry;
95 	}
96 	return NULL;
97 }
98 
99 /*
100  * Wrapper function to return a ib_mad_port_private structure or NULL
101  * for a device/port
102  */
103 static inline struct ib_mad_port_private *
104 ib_get_mad_port(struct ib_device *device, int port_num)
105 {
106 	struct ib_mad_port_private *entry;
107 	unsigned long flags;
108 
109 	spin_lock_irqsave(&ib_mad_port_list_lock, flags);
110 	entry = __ib_get_mad_port(device, port_num);
111 	spin_unlock_irqrestore(&ib_mad_port_list_lock, flags);
112 
113 	return entry;
114 }
115 
116 static inline u8 convert_mgmt_class(u8 mgmt_class)
117 {
118 	/* Alias IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE to 0 */
119 	return mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE ?
120 		0 : mgmt_class;
121 }
122 
123 static int get_spl_qp_index(enum ib_qp_type qp_type)
124 {
125 	switch (qp_type)
126 	{
127 	case IB_QPT_SMI:
128 		return 0;
129 	case IB_QPT_GSI:
130 		return 1;
131 	default:
132 		return -1;
133 	}
134 }
135 
136 static int vendor_class_index(u8 mgmt_class)
137 {
138 	return mgmt_class - IB_MGMT_CLASS_VENDOR_RANGE2_START;
139 }
140 
141 static int is_vendor_class(u8 mgmt_class)
142 {
143 	if ((mgmt_class < IB_MGMT_CLASS_VENDOR_RANGE2_START) ||
144 	    (mgmt_class > IB_MGMT_CLASS_VENDOR_RANGE2_END))
145 		return 0;
146 	return 1;
147 }
148 
149 static int is_vendor_oui(char *oui)
150 {
151 	if (oui[0] || oui[1] || oui[2])
152 		return 1;
153 	return 0;
154 }
155 
156 static int is_vendor_method_in_use(
157 		struct ib_mad_mgmt_vendor_class *vendor_class,
158 		struct ib_mad_reg_req *mad_reg_req)
159 {
160 	struct ib_mad_mgmt_method_table *method;
161 	int i;
162 
163 	for (i = 0; i < MAX_MGMT_OUI; i++) {
164 		if (!memcmp(vendor_class->oui[i], mad_reg_req->oui, 3)) {
165 			method = vendor_class->method_table[i];
166 			if (method) {
167 				if (method_in_use(&method, mad_reg_req))
168 					return 1;
169 				else
170 					break;
171 			}
172 		}
173 	}
174 	return 0;
175 }
176 
177 int ib_response_mad(struct ib_mad *mad)
178 {
179 	return ((mad->mad_hdr.method & IB_MGMT_METHOD_RESP) ||
180 		(mad->mad_hdr.method == IB_MGMT_METHOD_TRAP_REPRESS) ||
181 		((mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_BM) &&
182 		 (mad->mad_hdr.attr_mod & IB_BM_ATTR_MOD_RESP)));
183 }
184 EXPORT_SYMBOL(ib_response_mad);
185 
186 /*
187  * ib_register_mad_agent - Register to send/receive MADs
188  */
189 struct ib_mad_agent *ib_register_mad_agent(struct ib_device *device,
190 					   u8 port_num,
191 					   enum ib_qp_type qp_type,
192 					   struct ib_mad_reg_req *mad_reg_req,
193 					   u8 rmpp_version,
194 					   ib_mad_send_handler send_handler,
195 					   ib_mad_recv_handler recv_handler,
196 					   void *context)
197 {
198 	struct ib_mad_port_private *port_priv;
199 	struct ib_mad_agent *ret = ERR_PTR(-EINVAL);
200 	struct ib_mad_agent_private *mad_agent_priv;
201 	struct ib_mad_reg_req *reg_req = NULL;
202 	struct ib_mad_mgmt_class_table *class;
203 	struct ib_mad_mgmt_vendor_class_table *vendor;
204 	struct ib_mad_mgmt_vendor_class *vendor_class;
205 	struct ib_mad_mgmt_method_table *method;
206 	int ret2, qpn;
207 	unsigned long flags;
208 	u8 mgmt_class, vclass;
209 
210 	/* Validate parameters */
211 	qpn = get_spl_qp_index(qp_type);
212 	if (qpn == -1)
213 		goto error1;
214 
215 	if (rmpp_version && rmpp_version != IB_MGMT_RMPP_VERSION)
216 		goto error1;
217 
218 	/* Validate MAD registration request if supplied */
219 	if (mad_reg_req) {
220 		if (mad_reg_req->mgmt_class_version >= MAX_MGMT_VERSION)
221 			goto error1;
222 		if (!recv_handler)
223 			goto error1;
224 		if (mad_reg_req->mgmt_class >= MAX_MGMT_CLASS) {
225 			/*
226 			 * IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE is the only
227 			 * one in this range currently allowed
228 			 */
229 			if (mad_reg_req->mgmt_class !=
230 			    IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE)
231 				goto error1;
232 		} else if (mad_reg_req->mgmt_class == 0) {
233 			/*
234 			 * Class 0 is reserved in IBA and is used for
235 			 * aliasing of IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE
236 			 */
237 			goto error1;
238 		} else if (is_vendor_class(mad_reg_req->mgmt_class)) {
239 			/*
240 			 * If class is in "new" vendor range,
241 			 * ensure supplied OUI is not zero
242 			 */
243 			if (!is_vendor_oui(mad_reg_req->oui))
244 				goto error1;
245 		}
246 		/* Make sure class supplied is consistent with RMPP */
247 		if (!ib_is_mad_class_rmpp(mad_reg_req->mgmt_class)) {
248 			if (rmpp_version)
249 				goto error1;
250 		}
251 		/* Make sure class supplied is consistent with QP type */
252 		if (qp_type == IB_QPT_SMI) {
253 			if ((mad_reg_req->mgmt_class !=
254 					IB_MGMT_CLASS_SUBN_LID_ROUTED) &&
255 			    (mad_reg_req->mgmt_class !=
256 					IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE))
257 				goto error1;
258 		} else {
259 			if ((mad_reg_req->mgmt_class ==
260 					IB_MGMT_CLASS_SUBN_LID_ROUTED) ||
261 			    (mad_reg_req->mgmt_class ==
262 					IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE))
263 				goto error1;
264 		}
265 	} else {
266 		/* No registration request supplied */
267 		if (!send_handler)
268 			goto error1;
269 	}
270 
271 	/* Validate device and port */
272 	port_priv = ib_get_mad_port(device, port_num);
273 	if (!port_priv) {
274 		ret = ERR_PTR(-ENODEV);
275 		goto error1;
276 	}
277 
278 	/* Allocate structures */
279 	mad_agent_priv = kzalloc(sizeof *mad_agent_priv, GFP_KERNEL);
280 	if (!mad_agent_priv) {
281 		ret = ERR_PTR(-ENOMEM);
282 		goto error1;
283 	}
284 
285 	mad_agent_priv->agent.mr = ib_get_dma_mr(port_priv->qp_info[qpn].qp->pd,
286 						 IB_ACCESS_LOCAL_WRITE);
287 	if (IS_ERR(mad_agent_priv->agent.mr)) {
288 		ret = ERR_PTR(-ENOMEM);
289 		goto error2;
290 	}
291 
292 	if (mad_reg_req) {
293 		reg_req = kmalloc(sizeof *reg_req, GFP_KERNEL);
294 		if (!reg_req) {
295 			ret = ERR_PTR(-ENOMEM);
296 			goto error3;
297 		}
298 		/* Make a copy of the MAD registration request */
299 		memcpy(reg_req, mad_reg_req, sizeof *reg_req);
300 	}
301 
302 	/* Now, fill in the various structures */
303 	mad_agent_priv->qp_info = &port_priv->qp_info[qpn];
304 	mad_agent_priv->reg_req = reg_req;
305 	mad_agent_priv->agent.rmpp_version = rmpp_version;
306 	mad_agent_priv->agent.device = device;
307 	mad_agent_priv->agent.recv_handler = recv_handler;
308 	mad_agent_priv->agent.send_handler = send_handler;
309 	mad_agent_priv->agent.context = context;
310 	mad_agent_priv->agent.qp = port_priv->qp_info[qpn].qp;
311 	mad_agent_priv->agent.port_num = port_num;
312 	spin_lock_init(&mad_agent_priv->lock);
313 	INIT_LIST_HEAD(&mad_agent_priv->send_list);
314 	INIT_LIST_HEAD(&mad_agent_priv->wait_list);
315 	INIT_LIST_HEAD(&mad_agent_priv->done_list);
316 	INIT_LIST_HEAD(&mad_agent_priv->rmpp_list);
317 	INIT_DELAYED_WORK(&mad_agent_priv->timed_work, timeout_sends);
318 	INIT_LIST_HEAD(&mad_agent_priv->local_list);
319 	INIT_WORK(&mad_agent_priv->local_work, local_completions);
320 	atomic_set(&mad_agent_priv->refcount, 1);
321 	init_completion(&mad_agent_priv->comp);
322 
323 	spin_lock_irqsave(&port_priv->reg_lock, flags);
324 	mad_agent_priv->agent.hi_tid = ++ib_mad_client_id;
325 
326 	/*
327 	 * Make sure MAD registration (if supplied)
328 	 * is non overlapping with any existing ones
329 	 */
330 	if (mad_reg_req) {
331 		mgmt_class = convert_mgmt_class(mad_reg_req->mgmt_class);
332 		if (!is_vendor_class(mgmt_class)) {
333 			class = port_priv->version[mad_reg_req->
334 						   mgmt_class_version].class;
335 			if (class) {
336 				method = class->method_table[mgmt_class];
337 				if (method) {
338 					if (method_in_use(&method,
339 							   mad_reg_req))
340 						goto error4;
341 				}
342 			}
343 			ret2 = add_nonoui_reg_req(mad_reg_req, mad_agent_priv,
344 						  mgmt_class);
345 		} else {
346 			/* "New" vendor class range */
347 			vendor = port_priv->version[mad_reg_req->
348 						    mgmt_class_version].vendor;
349 			if (vendor) {
350 				vclass = vendor_class_index(mgmt_class);
351 				vendor_class = vendor->vendor_class[vclass];
352 				if (vendor_class) {
353 					if (is_vendor_method_in_use(
354 							vendor_class,
355 							mad_reg_req))
356 						goto error4;
357 				}
358 			}
359 			ret2 = add_oui_reg_req(mad_reg_req, mad_agent_priv);
360 		}
361 		if (ret2) {
362 			ret = ERR_PTR(ret2);
363 			goto error4;
364 		}
365 	}
366 
367 	/* Add mad agent into port's agent list */
368 	list_add_tail(&mad_agent_priv->agent_list, &port_priv->agent_list);
369 	spin_unlock_irqrestore(&port_priv->reg_lock, flags);
370 
371 	return &mad_agent_priv->agent;
372 
373 error4:
374 	spin_unlock_irqrestore(&port_priv->reg_lock, flags);
375 	kfree(reg_req);
376 error3:
377 	ib_dereg_mr(mad_agent_priv->agent.mr);
378 error2:
379 	kfree(mad_agent_priv);
380 error1:
381 	return ret;
382 }
383 EXPORT_SYMBOL(ib_register_mad_agent);
384 
385 static inline int is_snooping_sends(int mad_snoop_flags)
386 {
387 	return (mad_snoop_flags &
388 		(/*IB_MAD_SNOOP_POSTED_SENDS |
389 		 IB_MAD_SNOOP_RMPP_SENDS |*/
390 		 IB_MAD_SNOOP_SEND_COMPLETIONS /*|
391 		 IB_MAD_SNOOP_RMPP_SEND_COMPLETIONS*/));
392 }
393 
394 static inline int is_snooping_recvs(int mad_snoop_flags)
395 {
396 	return (mad_snoop_flags &
397 		(IB_MAD_SNOOP_RECVS /*|
398 		 IB_MAD_SNOOP_RMPP_RECVS*/));
399 }
400 
401 static int register_snoop_agent(struct ib_mad_qp_info *qp_info,
402 				struct ib_mad_snoop_private *mad_snoop_priv)
403 {
404 	struct ib_mad_snoop_private **new_snoop_table;
405 	unsigned long flags;
406 	int i;
407 
408 	spin_lock_irqsave(&qp_info->snoop_lock, flags);
409 	/* Check for empty slot in array. */
410 	for (i = 0; i < qp_info->snoop_table_size; i++)
411 		if (!qp_info->snoop_table[i])
412 			break;
413 
414 	if (i == qp_info->snoop_table_size) {
415 		/* Grow table. */
416 		new_snoop_table = krealloc(qp_info->snoop_table,
417 					   sizeof mad_snoop_priv *
418 					   (qp_info->snoop_table_size + 1),
419 					   GFP_ATOMIC);
420 		if (!new_snoop_table) {
421 			i = -ENOMEM;
422 			goto out;
423 		}
424 
425 		qp_info->snoop_table = new_snoop_table;
426 		qp_info->snoop_table_size++;
427 	}
428 	qp_info->snoop_table[i] = mad_snoop_priv;
429 	atomic_inc(&qp_info->snoop_count);
430 out:
431 	spin_unlock_irqrestore(&qp_info->snoop_lock, flags);
432 	return i;
433 }
434 
435 struct ib_mad_agent *ib_register_mad_snoop(struct ib_device *device,
436 					   u8 port_num,
437 					   enum ib_qp_type qp_type,
438 					   int mad_snoop_flags,
439 					   ib_mad_snoop_handler snoop_handler,
440 					   ib_mad_recv_handler recv_handler,
441 					   void *context)
442 {
443 	struct ib_mad_port_private *port_priv;
444 	struct ib_mad_agent *ret;
445 	struct ib_mad_snoop_private *mad_snoop_priv;
446 	int qpn;
447 
448 	/* Validate parameters */
449 	if ((is_snooping_sends(mad_snoop_flags) && !snoop_handler) ||
450 	    (is_snooping_recvs(mad_snoop_flags) && !recv_handler)) {
451 		ret = ERR_PTR(-EINVAL);
452 		goto error1;
453 	}
454 	qpn = get_spl_qp_index(qp_type);
455 	if (qpn == -1) {
456 		ret = ERR_PTR(-EINVAL);
457 		goto error1;
458 	}
459 	port_priv = ib_get_mad_port(device, port_num);
460 	if (!port_priv) {
461 		ret = ERR_PTR(-ENODEV);
462 		goto error1;
463 	}
464 	/* Allocate structures */
465 	mad_snoop_priv = kzalloc(sizeof *mad_snoop_priv, GFP_KERNEL);
466 	if (!mad_snoop_priv) {
467 		ret = ERR_PTR(-ENOMEM);
468 		goto error1;
469 	}
470 
471 	/* Now, fill in the various structures */
472 	mad_snoop_priv->qp_info = &port_priv->qp_info[qpn];
473 	mad_snoop_priv->agent.device = device;
474 	mad_snoop_priv->agent.recv_handler = recv_handler;
475 	mad_snoop_priv->agent.snoop_handler = snoop_handler;
476 	mad_snoop_priv->agent.context = context;
477 	mad_snoop_priv->agent.qp = port_priv->qp_info[qpn].qp;
478 	mad_snoop_priv->agent.port_num = port_num;
479 	mad_snoop_priv->mad_snoop_flags = mad_snoop_flags;
480 	init_completion(&mad_snoop_priv->comp);
481 	mad_snoop_priv->snoop_index = register_snoop_agent(
482 						&port_priv->qp_info[qpn],
483 						mad_snoop_priv);
484 	if (mad_snoop_priv->snoop_index < 0) {
485 		ret = ERR_PTR(mad_snoop_priv->snoop_index);
486 		goto error2;
487 	}
488 
489 	atomic_set(&mad_snoop_priv->refcount, 1);
490 	return &mad_snoop_priv->agent;
491 
492 error2:
493 	kfree(mad_snoop_priv);
494 error1:
495 	return ret;
496 }
497 EXPORT_SYMBOL(ib_register_mad_snoop);
498 
499 static inline void deref_mad_agent(struct ib_mad_agent_private *mad_agent_priv)
500 {
501 	if (atomic_dec_and_test(&mad_agent_priv->refcount))
502 		complete(&mad_agent_priv->comp);
503 }
504 
505 static inline void deref_snoop_agent(struct ib_mad_snoop_private *mad_snoop_priv)
506 {
507 	if (atomic_dec_and_test(&mad_snoop_priv->refcount))
508 		complete(&mad_snoop_priv->comp);
509 }
510 
511 static void unregister_mad_agent(struct ib_mad_agent_private *mad_agent_priv)
512 {
513 	struct ib_mad_port_private *port_priv;
514 	unsigned long flags;
515 
516 	/* Note that we could still be handling received MADs */
517 
518 	/*
519 	 * Canceling all sends results in dropping received response
520 	 * MADs, preventing us from queuing additional work
521 	 */
522 	cancel_mads(mad_agent_priv);
523 	port_priv = mad_agent_priv->qp_info->port_priv;
524 	cancel_delayed_work(&mad_agent_priv->timed_work);
525 
526 	spin_lock_irqsave(&port_priv->reg_lock, flags);
527 	remove_mad_reg_req(mad_agent_priv);
528 	list_del(&mad_agent_priv->agent_list);
529 	spin_unlock_irqrestore(&port_priv->reg_lock, flags);
530 
531 	flush_workqueue(port_priv->wq);
532 	ib_cancel_rmpp_recvs(mad_agent_priv);
533 
534 	deref_mad_agent(mad_agent_priv);
535 	wait_for_completion(&mad_agent_priv->comp);
536 
537 	kfree(mad_agent_priv->reg_req);
538 	ib_dereg_mr(mad_agent_priv->agent.mr);
539 	kfree(mad_agent_priv);
540 }
541 
542 static void unregister_mad_snoop(struct ib_mad_snoop_private *mad_snoop_priv)
543 {
544 	struct ib_mad_qp_info *qp_info;
545 	unsigned long flags;
546 
547 	qp_info = mad_snoop_priv->qp_info;
548 	spin_lock_irqsave(&qp_info->snoop_lock, flags);
549 	qp_info->snoop_table[mad_snoop_priv->snoop_index] = NULL;
550 	atomic_dec(&qp_info->snoop_count);
551 	spin_unlock_irqrestore(&qp_info->snoop_lock, flags);
552 
553 	deref_snoop_agent(mad_snoop_priv);
554 	wait_for_completion(&mad_snoop_priv->comp);
555 
556 	kfree(mad_snoop_priv);
557 }
558 
559 /*
560  * ib_unregister_mad_agent - Unregisters a client from using MAD services
561  */
562 int ib_unregister_mad_agent(struct ib_mad_agent *mad_agent)
563 {
564 	struct ib_mad_agent_private *mad_agent_priv;
565 	struct ib_mad_snoop_private *mad_snoop_priv;
566 
567 	/* If the TID is zero, the agent can only snoop. */
568 	if (mad_agent->hi_tid) {
569 		mad_agent_priv = container_of(mad_agent,
570 					      struct ib_mad_agent_private,
571 					      agent);
572 		unregister_mad_agent(mad_agent_priv);
573 	} else {
574 		mad_snoop_priv = container_of(mad_agent,
575 					      struct ib_mad_snoop_private,
576 					      agent);
577 		unregister_mad_snoop(mad_snoop_priv);
578 	}
579 	return 0;
580 }
581 EXPORT_SYMBOL(ib_unregister_mad_agent);
582 
583 static void dequeue_mad(struct ib_mad_list_head *mad_list)
584 {
585 	struct ib_mad_queue *mad_queue;
586 	unsigned long flags;
587 
588 	BUG_ON(!mad_list->mad_queue);
589 	mad_queue = mad_list->mad_queue;
590 	spin_lock_irqsave(&mad_queue->lock, flags);
591 	list_del(&mad_list->list);
592 	mad_queue->count--;
593 	spin_unlock_irqrestore(&mad_queue->lock, flags);
594 }
595 
596 static void snoop_send(struct ib_mad_qp_info *qp_info,
597 		       struct ib_mad_send_buf *send_buf,
598 		       struct ib_mad_send_wc *mad_send_wc,
599 		       int mad_snoop_flags)
600 {
601 	struct ib_mad_snoop_private *mad_snoop_priv;
602 	unsigned long flags;
603 	int i;
604 
605 	spin_lock_irqsave(&qp_info->snoop_lock, flags);
606 	for (i = 0; i < qp_info->snoop_table_size; i++) {
607 		mad_snoop_priv = qp_info->snoop_table[i];
608 		if (!mad_snoop_priv ||
609 		    !(mad_snoop_priv->mad_snoop_flags & mad_snoop_flags))
610 			continue;
611 
612 		atomic_inc(&mad_snoop_priv->refcount);
613 		spin_unlock_irqrestore(&qp_info->snoop_lock, flags);
614 		mad_snoop_priv->agent.snoop_handler(&mad_snoop_priv->agent,
615 						    send_buf, mad_send_wc);
616 		deref_snoop_agent(mad_snoop_priv);
617 		spin_lock_irqsave(&qp_info->snoop_lock, flags);
618 	}
619 	spin_unlock_irqrestore(&qp_info->snoop_lock, flags);
620 }
621 
622 static void snoop_recv(struct ib_mad_qp_info *qp_info,
623 		       struct ib_mad_recv_wc *mad_recv_wc,
624 		       int mad_snoop_flags)
625 {
626 	struct ib_mad_snoop_private *mad_snoop_priv;
627 	unsigned long flags;
628 	int i;
629 
630 	spin_lock_irqsave(&qp_info->snoop_lock, flags);
631 	for (i = 0; i < qp_info->snoop_table_size; i++) {
632 		mad_snoop_priv = qp_info->snoop_table[i];
633 		if (!mad_snoop_priv ||
634 		    !(mad_snoop_priv->mad_snoop_flags & mad_snoop_flags))
635 			continue;
636 
637 		atomic_inc(&mad_snoop_priv->refcount);
638 		spin_unlock_irqrestore(&qp_info->snoop_lock, flags);
639 		mad_snoop_priv->agent.recv_handler(&mad_snoop_priv->agent,
640 						   mad_recv_wc);
641 		deref_snoop_agent(mad_snoop_priv);
642 		spin_lock_irqsave(&qp_info->snoop_lock, flags);
643 	}
644 	spin_unlock_irqrestore(&qp_info->snoop_lock, flags);
645 }
646 
647 static void build_smp_wc(struct ib_qp *qp,
648 			 u64 wr_id, u16 slid, u16 pkey_index, u8 port_num,
649 			 struct ib_wc *wc)
650 {
651 	memset(wc, 0, sizeof *wc);
652 	wc->wr_id = wr_id;
653 	wc->status = IB_WC_SUCCESS;
654 	wc->opcode = IB_WC_RECV;
655 	wc->pkey_index = pkey_index;
656 	wc->byte_len = sizeof(struct ib_mad) + sizeof(struct ib_grh);
657 	wc->src_qp = IB_QP0;
658 	wc->qp = qp;
659 	wc->slid = slid;
660 	wc->sl = 0;
661 	wc->dlid_path_bits = 0;
662 	wc->port_num = port_num;
663 }
664 
665 /*
666  * Return 0 if SMP is to be sent
667  * Return 1 if SMP was consumed locally (whether or not solicited)
668  * Return < 0 if error
669  */
670 static int handle_outgoing_dr_smp(struct ib_mad_agent_private *mad_agent_priv,
671 				  struct ib_mad_send_wr_private *mad_send_wr)
672 {
673 	int ret = 0;
674 	struct ib_smp *smp = mad_send_wr->send_buf.mad;
675 	unsigned long flags;
676 	struct ib_mad_local_private *local;
677 	struct ib_mad_private *mad_priv;
678 	struct ib_mad_port_private *port_priv;
679 	struct ib_mad_agent_private *recv_mad_agent = NULL;
680 	struct ib_device *device = mad_agent_priv->agent.device;
681 	u8 port_num;
682 	struct ib_wc mad_wc;
683 	struct ib_send_wr *send_wr = &mad_send_wr->send_wr;
684 
685 	if (device->node_type == RDMA_NODE_IB_SWITCH &&
686 	    smp->mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE)
687 		port_num = send_wr->wr.ud.port_num;
688 	else
689 		port_num = mad_agent_priv->agent.port_num;
690 
691 	/*
692 	 * Directed route handling starts if the initial LID routed part of
693 	 * a request or the ending LID routed part of a response is empty.
694 	 * If we are at the start of the LID routed part, don't update the
695 	 * hop_ptr or hop_cnt.  See section 14.2.2, Vol 1 IB spec.
696 	 */
697 	if ((ib_get_smp_direction(smp) ? smp->dr_dlid : smp->dr_slid) ==
698 	     IB_LID_PERMISSIVE &&
699 	     smi_handle_dr_smp_send(smp, device->node_type, port_num) ==
700 	     IB_SMI_DISCARD) {
701 		ret = -EINVAL;
702 		printk(KERN_ERR PFX "Invalid directed route\n");
703 		goto out;
704 	}
705 
706 	/* Check to post send on QP or process locally */
707 	if (smi_check_local_smp(smp, device) == IB_SMI_DISCARD &&
708 	    smi_check_local_returning_smp(smp, device) == IB_SMI_DISCARD)
709 		goto out;
710 
711 	local = kmalloc(sizeof *local, GFP_ATOMIC);
712 	if (!local) {
713 		ret = -ENOMEM;
714 		printk(KERN_ERR PFX "No memory for ib_mad_local_private\n");
715 		goto out;
716 	}
717 	local->mad_priv = NULL;
718 	local->recv_mad_agent = NULL;
719 	mad_priv = kmem_cache_alloc(ib_mad_cache, GFP_ATOMIC);
720 	if (!mad_priv) {
721 		ret = -ENOMEM;
722 		printk(KERN_ERR PFX "No memory for local response MAD\n");
723 		kfree(local);
724 		goto out;
725 	}
726 
727 	build_smp_wc(mad_agent_priv->agent.qp,
728 		     send_wr->wr_id, be16_to_cpu(smp->dr_slid),
729 		     send_wr->wr.ud.pkey_index,
730 		     send_wr->wr.ud.port_num, &mad_wc);
731 
732 	/* No GRH for DR SMP */
733 	ret = device->process_mad(device, 0, port_num, &mad_wc, NULL,
734 				  (struct ib_mad *)smp,
735 				  (struct ib_mad *)&mad_priv->mad);
736 	switch (ret)
737 	{
738 	case IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_REPLY:
739 		if (ib_response_mad(&mad_priv->mad.mad) &&
740 		    mad_agent_priv->agent.recv_handler) {
741 			local->mad_priv = mad_priv;
742 			local->recv_mad_agent = mad_agent_priv;
743 			/*
744 			 * Reference MAD agent until receive
745 			 * side of local completion handled
746 			 */
747 			atomic_inc(&mad_agent_priv->refcount);
748 		} else
749 			kmem_cache_free(ib_mad_cache, mad_priv);
750 		break;
751 	case IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_CONSUMED:
752 		kmem_cache_free(ib_mad_cache, mad_priv);
753 		break;
754 	case IB_MAD_RESULT_SUCCESS:
755 		/* Treat like an incoming receive MAD */
756 		port_priv = ib_get_mad_port(mad_agent_priv->agent.device,
757 					    mad_agent_priv->agent.port_num);
758 		if (port_priv) {
759 			memcpy(&mad_priv->mad.mad, smp, sizeof(struct ib_mad));
760 			recv_mad_agent = find_mad_agent(port_priv,
761 						        &mad_priv->mad.mad);
762 		}
763 		if (!port_priv || !recv_mad_agent) {
764 			/*
765 			 * No receiving agent so drop packet and
766 			 * generate send completion.
767 			 */
768 			kmem_cache_free(ib_mad_cache, mad_priv);
769 			break;
770 		}
771 		local->mad_priv = mad_priv;
772 		local->recv_mad_agent = recv_mad_agent;
773 		break;
774 	default:
775 		kmem_cache_free(ib_mad_cache, mad_priv);
776 		kfree(local);
777 		ret = -EINVAL;
778 		goto out;
779 	}
780 
781 	local->mad_send_wr = mad_send_wr;
782 	/* Reference MAD agent until send side of local completion handled */
783 	atomic_inc(&mad_agent_priv->refcount);
784 	/* Queue local completion to local list */
785 	spin_lock_irqsave(&mad_agent_priv->lock, flags);
786 	list_add_tail(&local->completion_list, &mad_agent_priv->local_list);
787 	spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
788 	queue_work(mad_agent_priv->qp_info->port_priv->wq,
789 		   &mad_agent_priv->local_work);
790 	ret = 1;
791 out:
792 	return ret;
793 }
794 
795 static int get_pad_size(int hdr_len, int data_len)
796 {
797 	int seg_size, pad;
798 
799 	seg_size = sizeof(struct ib_mad) - hdr_len;
800 	if (data_len && seg_size) {
801 		pad = seg_size - data_len % seg_size;
802 		return pad == seg_size ? 0 : pad;
803 	} else
804 		return seg_size;
805 }
806 
807 static void free_send_rmpp_list(struct ib_mad_send_wr_private *mad_send_wr)
808 {
809 	struct ib_rmpp_segment *s, *t;
810 
811 	list_for_each_entry_safe(s, t, &mad_send_wr->rmpp_list, list) {
812 		list_del(&s->list);
813 		kfree(s);
814 	}
815 }
816 
817 static int alloc_send_rmpp_list(struct ib_mad_send_wr_private *send_wr,
818 				gfp_t gfp_mask)
819 {
820 	struct ib_mad_send_buf *send_buf = &send_wr->send_buf;
821 	struct ib_rmpp_mad *rmpp_mad = send_buf->mad;
822 	struct ib_rmpp_segment *seg = NULL;
823 	int left, seg_size, pad;
824 
825 	send_buf->seg_size = sizeof (struct ib_mad) - send_buf->hdr_len;
826 	seg_size = send_buf->seg_size;
827 	pad = send_wr->pad;
828 
829 	/* Allocate data segments. */
830 	for (left = send_buf->data_len + pad; left > 0; left -= seg_size) {
831 		seg = kmalloc(sizeof (*seg) + seg_size, gfp_mask);
832 		if (!seg) {
833 			printk(KERN_ERR "alloc_send_rmpp_segs: RMPP mem "
834 			       "alloc failed for len %zd, gfp %#x\n",
835 			       sizeof (*seg) + seg_size, gfp_mask);
836 			free_send_rmpp_list(send_wr);
837 			return -ENOMEM;
838 		}
839 		seg->num = ++send_buf->seg_count;
840 		list_add_tail(&seg->list, &send_wr->rmpp_list);
841 	}
842 
843 	/* Zero any padding */
844 	if (pad)
845 		memset(seg->data + seg_size - pad, 0, pad);
846 
847 	rmpp_mad->rmpp_hdr.rmpp_version = send_wr->mad_agent_priv->
848 					  agent.rmpp_version;
849 	rmpp_mad->rmpp_hdr.rmpp_type = IB_MGMT_RMPP_TYPE_DATA;
850 	ib_set_rmpp_flags(&rmpp_mad->rmpp_hdr, IB_MGMT_RMPP_FLAG_ACTIVE);
851 
852 	send_wr->cur_seg = container_of(send_wr->rmpp_list.next,
853 					struct ib_rmpp_segment, list);
854 	send_wr->last_ack_seg = send_wr->cur_seg;
855 	return 0;
856 }
857 
858 struct ib_mad_send_buf * ib_create_send_mad(struct ib_mad_agent *mad_agent,
859 					    u32 remote_qpn, u16 pkey_index,
860 					    int rmpp_active,
861 					    int hdr_len, int data_len,
862 					    gfp_t gfp_mask)
863 {
864 	struct ib_mad_agent_private *mad_agent_priv;
865 	struct ib_mad_send_wr_private *mad_send_wr;
866 	int pad, message_size, ret, size;
867 	void *buf;
868 
869 	mad_agent_priv = container_of(mad_agent, struct ib_mad_agent_private,
870 				      agent);
871 	pad = get_pad_size(hdr_len, data_len);
872 	message_size = hdr_len + data_len + pad;
873 
874 	if ((!mad_agent->rmpp_version &&
875 	     (rmpp_active || message_size > sizeof(struct ib_mad))) ||
876 	    (!rmpp_active && message_size > sizeof(struct ib_mad)))
877 		return ERR_PTR(-EINVAL);
878 
879 	size = rmpp_active ? hdr_len : sizeof(struct ib_mad);
880 	buf = kzalloc(sizeof *mad_send_wr + size, gfp_mask);
881 	if (!buf)
882 		return ERR_PTR(-ENOMEM);
883 
884 	mad_send_wr = buf + size;
885 	INIT_LIST_HEAD(&mad_send_wr->rmpp_list);
886 	mad_send_wr->send_buf.mad = buf;
887 	mad_send_wr->send_buf.hdr_len = hdr_len;
888 	mad_send_wr->send_buf.data_len = data_len;
889 	mad_send_wr->pad = pad;
890 
891 	mad_send_wr->mad_agent_priv = mad_agent_priv;
892 	mad_send_wr->sg_list[0].length = hdr_len;
893 	mad_send_wr->sg_list[0].lkey = mad_agent->mr->lkey;
894 	mad_send_wr->sg_list[1].length = sizeof(struct ib_mad) - hdr_len;
895 	mad_send_wr->sg_list[1].lkey = mad_agent->mr->lkey;
896 
897 	mad_send_wr->send_wr.wr_id = (unsigned long) mad_send_wr;
898 	mad_send_wr->send_wr.sg_list = mad_send_wr->sg_list;
899 	mad_send_wr->send_wr.num_sge = 2;
900 	mad_send_wr->send_wr.opcode = IB_WR_SEND;
901 	mad_send_wr->send_wr.send_flags = IB_SEND_SIGNALED;
902 	mad_send_wr->send_wr.wr.ud.remote_qpn = remote_qpn;
903 	mad_send_wr->send_wr.wr.ud.remote_qkey = IB_QP_SET_QKEY;
904 	mad_send_wr->send_wr.wr.ud.pkey_index = pkey_index;
905 
906 	if (rmpp_active) {
907 		ret = alloc_send_rmpp_list(mad_send_wr, gfp_mask);
908 		if (ret) {
909 			kfree(buf);
910 			return ERR_PTR(ret);
911 		}
912 	}
913 
914 	mad_send_wr->send_buf.mad_agent = mad_agent;
915 	atomic_inc(&mad_agent_priv->refcount);
916 	return &mad_send_wr->send_buf;
917 }
918 EXPORT_SYMBOL(ib_create_send_mad);
919 
920 int ib_get_mad_data_offset(u8 mgmt_class)
921 {
922 	if (mgmt_class == IB_MGMT_CLASS_SUBN_ADM)
923 		return IB_MGMT_SA_HDR;
924 	else if ((mgmt_class == IB_MGMT_CLASS_DEVICE_MGMT) ||
925 		 (mgmt_class == IB_MGMT_CLASS_DEVICE_ADM) ||
926 		 (mgmt_class == IB_MGMT_CLASS_BIS))
927 		return IB_MGMT_DEVICE_HDR;
928 	else if ((mgmt_class >= IB_MGMT_CLASS_VENDOR_RANGE2_START) &&
929 		 (mgmt_class <= IB_MGMT_CLASS_VENDOR_RANGE2_END))
930 		return IB_MGMT_VENDOR_HDR;
931 	else
932 		return IB_MGMT_MAD_HDR;
933 }
934 EXPORT_SYMBOL(ib_get_mad_data_offset);
935 
936 int ib_is_mad_class_rmpp(u8 mgmt_class)
937 {
938 	if ((mgmt_class == IB_MGMT_CLASS_SUBN_ADM) ||
939 	    (mgmt_class == IB_MGMT_CLASS_DEVICE_MGMT) ||
940 	    (mgmt_class == IB_MGMT_CLASS_DEVICE_ADM) ||
941 	    (mgmt_class == IB_MGMT_CLASS_BIS) ||
942 	    ((mgmt_class >= IB_MGMT_CLASS_VENDOR_RANGE2_START) &&
943 	     (mgmt_class <= IB_MGMT_CLASS_VENDOR_RANGE2_END)))
944 		return 1;
945 	return 0;
946 }
947 EXPORT_SYMBOL(ib_is_mad_class_rmpp);
948 
949 void *ib_get_rmpp_segment(struct ib_mad_send_buf *send_buf, int seg_num)
950 {
951 	struct ib_mad_send_wr_private *mad_send_wr;
952 	struct list_head *list;
953 
954 	mad_send_wr = container_of(send_buf, struct ib_mad_send_wr_private,
955 				   send_buf);
956 	list = &mad_send_wr->cur_seg->list;
957 
958 	if (mad_send_wr->cur_seg->num < seg_num) {
959 		list_for_each_entry(mad_send_wr->cur_seg, list, list)
960 			if (mad_send_wr->cur_seg->num == seg_num)
961 				break;
962 	} else if (mad_send_wr->cur_seg->num > seg_num) {
963 		list_for_each_entry_reverse(mad_send_wr->cur_seg, list, list)
964 			if (mad_send_wr->cur_seg->num == seg_num)
965 				break;
966 	}
967 	return mad_send_wr->cur_seg->data;
968 }
969 EXPORT_SYMBOL(ib_get_rmpp_segment);
970 
971 static inline void *ib_get_payload(struct ib_mad_send_wr_private *mad_send_wr)
972 {
973 	if (mad_send_wr->send_buf.seg_count)
974 		return ib_get_rmpp_segment(&mad_send_wr->send_buf,
975 					   mad_send_wr->seg_num);
976 	else
977 		return mad_send_wr->send_buf.mad +
978 		       mad_send_wr->send_buf.hdr_len;
979 }
980 
981 void ib_free_send_mad(struct ib_mad_send_buf *send_buf)
982 {
983 	struct ib_mad_agent_private *mad_agent_priv;
984 	struct ib_mad_send_wr_private *mad_send_wr;
985 
986 	mad_agent_priv = container_of(send_buf->mad_agent,
987 				      struct ib_mad_agent_private, agent);
988 	mad_send_wr = container_of(send_buf, struct ib_mad_send_wr_private,
989 				   send_buf);
990 
991 	free_send_rmpp_list(mad_send_wr);
992 	kfree(send_buf->mad);
993 	deref_mad_agent(mad_agent_priv);
994 }
995 EXPORT_SYMBOL(ib_free_send_mad);
996 
997 int ib_send_mad(struct ib_mad_send_wr_private *mad_send_wr)
998 {
999 	struct ib_mad_qp_info *qp_info;
1000 	struct list_head *list;
1001 	struct ib_send_wr *bad_send_wr;
1002 	struct ib_mad_agent *mad_agent;
1003 	struct ib_sge *sge;
1004 	unsigned long flags;
1005 	int ret;
1006 
1007 	/* Set WR ID to find mad_send_wr upon completion */
1008 	qp_info = mad_send_wr->mad_agent_priv->qp_info;
1009 	mad_send_wr->send_wr.wr_id = (unsigned long)&mad_send_wr->mad_list;
1010 	mad_send_wr->mad_list.mad_queue = &qp_info->send_queue;
1011 
1012 	mad_agent = mad_send_wr->send_buf.mad_agent;
1013 	sge = mad_send_wr->sg_list;
1014 	sge[0].addr = ib_dma_map_single(mad_agent->device,
1015 					mad_send_wr->send_buf.mad,
1016 					sge[0].length,
1017 					DMA_TO_DEVICE);
1018 	mad_send_wr->header_mapping = sge[0].addr;
1019 
1020 	sge[1].addr = ib_dma_map_single(mad_agent->device,
1021 					ib_get_payload(mad_send_wr),
1022 					sge[1].length,
1023 					DMA_TO_DEVICE);
1024 	mad_send_wr->payload_mapping = sge[1].addr;
1025 
1026 	spin_lock_irqsave(&qp_info->send_queue.lock, flags);
1027 	if (qp_info->send_queue.count < qp_info->send_queue.max_active) {
1028 		ret = ib_post_send(mad_agent->qp, &mad_send_wr->send_wr,
1029 				   &bad_send_wr);
1030 		list = &qp_info->send_queue.list;
1031 	} else {
1032 		ret = 0;
1033 		list = &qp_info->overflow_list;
1034 	}
1035 
1036 	if (!ret) {
1037 		qp_info->send_queue.count++;
1038 		list_add_tail(&mad_send_wr->mad_list.list, list);
1039 	}
1040 	spin_unlock_irqrestore(&qp_info->send_queue.lock, flags);
1041 	if (ret) {
1042 		ib_dma_unmap_single(mad_agent->device,
1043 				    mad_send_wr->header_mapping,
1044 				    sge[0].length, DMA_TO_DEVICE);
1045 		ib_dma_unmap_single(mad_agent->device,
1046 				    mad_send_wr->payload_mapping,
1047 				    sge[1].length, DMA_TO_DEVICE);
1048 	}
1049 	return ret;
1050 }
1051 
1052 /*
1053  * ib_post_send_mad - Posts MAD(s) to the send queue of the QP associated
1054  *  with the registered client
1055  */
1056 int ib_post_send_mad(struct ib_mad_send_buf *send_buf,
1057 		     struct ib_mad_send_buf **bad_send_buf)
1058 {
1059 	struct ib_mad_agent_private *mad_agent_priv;
1060 	struct ib_mad_send_buf *next_send_buf;
1061 	struct ib_mad_send_wr_private *mad_send_wr;
1062 	unsigned long flags;
1063 	int ret = -EINVAL;
1064 
1065 	/* Walk list of send WRs and post each on send list */
1066 	for (; send_buf; send_buf = next_send_buf) {
1067 
1068 		mad_send_wr = container_of(send_buf,
1069 					   struct ib_mad_send_wr_private,
1070 					   send_buf);
1071 		mad_agent_priv = mad_send_wr->mad_agent_priv;
1072 
1073 		if (!send_buf->mad_agent->send_handler ||
1074 		    (send_buf->timeout_ms &&
1075 		     !send_buf->mad_agent->recv_handler)) {
1076 			ret = -EINVAL;
1077 			goto error;
1078 		}
1079 
1080 		if (!ib_is_mad_class_rmpp(((struct ib_mad_hdr *) send_buf->mad)->mgmt_class)) {
1081 			if (mad_agent_priv->agent.rmpp_version) {
1082 				ret = -EINVAL;
1083 				goto error;
1084 			}
1085 		}
1086 
1087 		/*
1088 		 * Save pointer to next work request to post in case the
1089 		 * current one completes, and the user modifies the work
1090 		 * request associated with the completion
1091 		 */
1092 		next_send_buf = send_buf->next;
1093 		mad_send_wr->send_wr.wr.ud.ah = send_buf->ah;
1094 
1095 		if (((struct ib_mad_hdr *) send_buf->mad)->mgmt_class ==
1096 		    IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE) {
1097 			ret = handle_outgoing_dr_smp(mad_agent_priv,
1098 						     mad_send_wr);
1099 			if (ret < 0)		/* error */
1100 				goto error;
1101 			else if (ret == 1)	/* locally consumed */
1102 				continue;
1103 		}
1104 
1105 		mad_send_wr->tid = ((struct ib_mad_hdr *) send_buf->mad)->tid;
1106 		/* Timeout will be updated after send completes */
1107 		mad_send_wr->timeout = msecs_to_jiffies(send_buf->timeout_ms);
1108 		mad_send_wr->max_retries = send_buf->retries;
1109 		mad_send_wr->retries_left = send_buf->retries;
1110 		send_buf->retries = 0;
1111 		/* Reference for work request to QP + response */
1112 		mad_send_wr->refcount = 1 + (mad_send_wr->timeout > 0);
1113 		mad_send_wr->status = IB_WC_SUCCESS;
1114 
1115 		/* Reference MAD agent until send completes */
1116 		atomic_inc(&mad_agent_priv->refcount);
1117 		spin_lock_irqsave(&mad_agent_priv->lock, flags);
1118 		list_add_tail(&mad_send_wr->agent_list,
1119 			      &mad_agent_priv->send_list);
1120 		spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
1121 
1122 		if (mad_agent_priv->agent.rmpp_version) {
1123 			ret = ib_send_rmpp_mad(mad_send_wr);
1124 			if (ret >= 0 && ret != IB_RMPP_RESULT_CONSUMED)
1125 				ret = ib_send_mad(mad_send_wr);
1126 		} else
1127 			ret = ib_send_mad(mad_send_wr);
1128 		if (ret < 0) {
1129 			/* Fail send request */
1130 			spin_lock_irqsave(&mad_agent_priv->lock, flags);
1131 			list_del(&mad_send_wr->agent_list);
1132 			spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
1133 			atomic_dec(&mad_agent_priv->refcount);
1134 			goto error;
1135 		}
1136 	}
1137 	return 0;
1138 error:
1139 	if (bad_send_buf)
1140 		*bad_send_buf = send_buf;
1141 	return ret;
1142 }
1143 EXPORT_SYMBOL(ib_post_send_mad);
1144 
1145 /*
1146  * ib_free_recv_mad - Returns data buffers used to receive
1147  *  a MAD to the access layer
1148  */
1149 void ib_free_recv_mad(struct ib_mad_recv_wc *mad_recv_wc)
1150 {
1151 	struct ib_mad_recv_buf *mad_recv_buf, *temp_recv_buf;
1152 	struct ib_mad_private_header *mad_priv_hdr;
1153 	struct ib_mad_private *priv;
1154 	struct list_head free_list;
1155 
1156 	INIT_LIST_HEAD(&free_list);
1157 	list_splice_init(&mad_recv_wc->rmpp_list, &free_list);
1158 
1159 	list_for_each_entry_safe(mad_recv_buf, temp_recv_buf,
1160 					&free_list, list) {
1161 		mad_recv_wc = container_of(mad_recv_buf, struct ib_mad_recv_wc,
1162 					   recv_buf);
1163 		mad_priv_hdr = container_of(mad_recv_wc,
1164 					    struct ib_mad_private_header,
1165 					    recv_wc);
1166 		priv = container_of(mad_priv_hdr, struct ib_mad_private,
1167 				    header);
1168 		kmem_cache_free(ib_mad_cache, priv);
1169 	}
1170 }
1171 EXPORT_SYMBOL(ib_free_recv_mad);
1172 
1173 struct ib_mad_agent *ib_redirect_mad_qp(struct ib_qp *qp,
1174 					u8 rmpp_version,
1175 					ib_mad_send_handler send_handler,
1176 					ib_mad_recv_handler recv_handler,
1177 					void *context)
1178 {
1179 	return ERR_PTR(-EINVAL);	/* XXX: for now */
1180 }
1181 EXPORT_SYMBOL(ib_redirect_mad_qp);
1182 
1183 int ib_process_mad_wc(struct ib_mad_agent *mad_agent,
1184 		      struct ib_wc *wc)
1185 {
1186 	printk(KERN_ERR PFX "ib_process_mad_wc() not implemented yet\n");
1187 	return 0;
1188 }
1189 EXPORT_SYMBOL(ib_process_mad_wc);
1190 
1191 static int method_in_use(struct ib_mad_mgmt_method_table **method,
1192 			 struct ib_mad_reg_req *mad_reg_req)
1193 {
1194 	int i;
1195 
1196 	for (i = find_first_bit(mad_reg_req->method_mask, IB_MGMT_MAX_METHODS);
1197 	     i < IB_MGMT_MAX_METHODS;
1198 	     i = find_next_bit(mad_reg_req->method_mask, IB_MGMT_MAX_METHODS,
1199 			       1+i)) {
1200 		if ((*method)->agent[i]) {
1201 			printk(KERN_ERR PFX "Method %d already in use\n", i);
1202 			return -EINVAL;
1203 		}
1204 	}
1205 	return 0;
1206 }
1207 
1208 static int allocate_method_table(struct ib_mad_mgmt_method_table **method)
1209 {
1210 	/* Allocate management method table */
1211 	*method = kzalloc(sizeof **method, GFP_ATOMIC);
1212 	if (!*method) {
1213 		printk(KERN_ERR PFX "No memory for "
1214 		       "ib_mad_mgmt_method_table\n");
1215 		return -ENOMEM;
1216 	}
1217 
1218 	return 0;
1219 }
1220 
1221 /*
1222  * Check to see if there are any methods still in use
1223  */
1224 static int check_method_table(struct ib_mad_mgmt_method_table *method)
1225 {
1226 	int i;
1227 
1228 	for (i = 0; i < IB_MGMT_MAX_METHODS; i++)
1229 		if (method->agent[i])
1230 			return 1;
1231 	return 0;
1232 }
1233 
1234 /*
1235  * Check to see if there are any method tables for this class still in use
1236  */
1237 static int check_class_table(struct ib_mad_mgmt_class_table *class)
1238 {
1239 	int i;
1240 
1241 	for (i = 0; i < MAX_MGMT_CLASS; i++)
1242 		if (class->method_table[i])
1243 			return 1;
1244 	return 0;
1245 }
1246 
1247 static int check_vendor_class(struct ib_mad_mgmt_vendor_class *vendor_class)
1248 {
1249 	int i;
1250 
1251 	for (i = 0; i < MAX_MGMT_OUI; i++)
1252 		if (vendor_class->method_table[i])
1253 			return 1;
1254 	return 0;
1255 }
1256 
1257 static int find_vendor_oui(struct ib_mad_mgmt_vendor_class *vendor_class,
1258 			   char *oui)
1259 {
1260 	int i;
1261 
1262 	for (i = 0; i < MAX_MGMT_OUI; i++)
1263 		/* Is there matching OUI for this vendor class ? */
1264 		if (!memcmp(vendor_class->oui[i], oui, 3))
1265 			return i;
1266 
1267 	return -1;
1268 }
1269 
1270 static int check_vendor_table(struct ib_mad_mgmt_vendor_class_table *vendor)
1271 {
1272 	int i;
1273 
1274 	for (i = 0; i < MAX_MGMT_VENDOR_RANGE2; i++)
1275 		if (vendor->vendor_class[i])
1276 			return 1;
1277 
1278 	return 0;
1279 }
1280 
1281 static void remove_methods_mad_agent(struct ib_mad_mgmt_method_table *method,
1282 				     struct ib_mad_agent_private *agent)
1283 {
1284 	int i;
1285 
1286 	/* Remove any methods for this mad agent */
1287 	for (i = 0; i < IB_MGMT_MAX_METHODS; i++) {
1288 		if (method->agent[i] == agent) {
1289 			method->agent[i] = NULL;
1290 		}
1291 	}
1292 }
1293 
1294 static int add_nonoui_reg_req(struct ib_mad_reg_req *mad_reg_req,
1295 			      struct ib_mad_agent_private *agent_priv,
1296 			      u8 mgmt_class)
1297 {
1298 	struct ib_mad_port_private *port_priv;
1299 	struct ib_mad_mgmt_class_table **class;
1300 	struct ib_mad_mgmt_method_table **method;
1301 	int i, ret;
1302 
1303 	port_priv = agent_priv->qp_info->port_priv;
1304 	class = &port_priv->version[mad_reg_req->mgmt_class_version].class;
1305 	if (!*class) {
1306 		/* Allocate management class table for "new" class version */
1307 		*class = kzalloc(sizeof **class, GFP_ATOMIC);
1308 		if (!*class) {
1309 			printk(KERN_ERR PFX "No memory for "
1310 			       "ib_mad_mgmt_class_table\n");
1311 			ret = -ENOMEM;
1312 			goto error1;
1313 		}
1314 
1315 		/* Allocate method table for this management class */
1316 		method = &(*class)->method_table[mgmt_class];
1317 		if ((ret = allocate_method_table(method)))
1318 			goto error2;
1319 	} else {
1320 		method = &(*class)->method_table[mgmt_class];
1321 		if (!*method) {
1322 			/* Allocate method table for this management class */
1323 			if ((ret = allocate_method_table(method)))
1324 				goto error1;
1325 		}
1326 	}
1327 
1328 	/* Now, make sure methods are not already in use */
1329 	if (method_in_use(method, mad_reg_req))
1330 		goto error3;
1331 
1332 	/* Finally, add in methods being registered */
1333 	for (i = find_first_bit(mad_reg_req->method_mask,
1334 				IB_MGMT_MAX_METHODS);
1335 	     i < IB_MGMT_MAX_METHODS;
1336 	     i = find_next_bit(mad_reg_req->method_mask, IB_MGMT_MAX_METHODS,
1337 			       1+i)) {
1338 		(*method)->agent[i] = agent_priv;
1339 	}
1340 	return 0;
1341 
1342 error3:
1343 	/* Remove any methods for this mad agent */
1344 	remove_methods_mad_agent(*method, agent_priv);
1345 	/* Now, check to see if there are any methods in use */
1346 	if (!check_method_table(*method)) {
1347 		/* If not, release management method table */
1348 		kfree(*method);
1349 		*method = NULL;
1350 	}
1351 	ret = -EINVAL;
1352 	goto error1;
1353 error2:
1354 	kfree(*class);
1355 	*class = NULL;
1356 error1:
1357 	return ret;
1358 }
1359 
1360 static int add_oui_reg_req(struct ib_mad_reg_req *mad_reg_req,
1361 			   struct ib_mad_agent_private *agent_priv)
1362 {
1363 	struct ib_mad_port_private *port_priv;
1364 	struct ib_mad_mgmt_vendor_class_table **vendor_table;
1365 	struct ib_mad_mgmt_vendor_class_table *vendor = NULL;
1366 	struct ib_mad_mgmt_vendor_class *vendor_class = NULL;
1367 	struct ib_mad_mgmt_method_table **method;
1368 	int i, ret = -ENOMEM;
1369 	u8 vclass;
1370 
1371 	/* "New" vendor (with OUI) class */
1372 	vclass = vendor_class_index(mad_reg_req->mgmt_class);
1373 	port_priv = agent_priv->qp_info->port_priv;
1374 	vendor_table = &port_priv->version[
1375 				mad_reg_req->mgmt_class_version].vendor;
1376 	if (!*vendor_table) {
1377 		/* Allocate mgmt vendor class table for "new" class version */
1378 		vendor = kzalloc(sizeof *vendor, GFP_ATOMIC);
1379 		if (!vendor) {
1380 			printk(KERN_ERR PFX "No memory for "
1381 			       "ib_mad_mgmt_vendor_class_table\n");
1382 			goto error1;
1383 		}
1384 
1385 		*vendor_table = vendor;
1386 	}
1387 	if (!(*vendor_table)->vendor_class[vclass]) {
1388 		/* Allocate table for this management vendor class */
1389 		vendor_class = kzalloc(sizeof *vendor_class, GFP_ATOMIC);
1390 		if (!vendor_class) {
1391 			printk(KERN_ERR PFX "No memory for "
1392 			       "ib_mad_mgmt_vendor_class\n");
1393 			goto error2;
1394 		}
1395 
1396 		(*vendor_table)->vendor_class[vclass] = vendor_class;
1397 	}
1398 	for (i = 0; i < MAX_MGMT_OUI; i++) {
1399 		/* Is there matching OUI for this vendor class ? */
1400 		if (!memcmp((*vendor_table)->vendor_class[vclass]->oui[i],
1401 			    mad_reg_req->oui, 3)) {
1402 			method = &(*vendor_table)->vendor_class[
1403 						vclass]->method_table[i];
1404 			BUG_ON(!*method);
1405 			goto check_in_use;
1406 		}
1407 	}
1408 	for (i = 0; i < MAX_MGMT_OUI; i++) {
1409 		/* OUI slot available ? */
1410 		if (!is_vendor_oui((*vendor_table)->vendor_class[
1411 				vclass]->oui[i])) {
1412 			method = &(*vendor_table)->vendor_class[
1413 				vclass]->method_table[i];
1414 			BUG_ON(*method);
1415 			/* Allocate method table for this OUI */
1416 			if ((ret = allocate_method_table(method)))
1417 				goto error3;
1418 			memcpy((*vendor_table)->vendor_class[vclass]->oui[i],
1419 			       mad_reg_req->oui, 3);
1420 			goto check_in_use;
1421 		}
1422 	}
1423 	printk(KERN_ERR PFX "All OUI slots in use\n");
1424 	goto error3;
1425 
1426 check_in_use:
1427 	/* Now, make sure methods are not already in use */
1428 	if (method_in_use(method, mad_reg_req))
1429 		goto error4;
1430 
1431 	/* Finally, add in methods being registered */
1432 	for (i = find_first_bit(mad_reg_req->method_mask,
1433 				IB_MGMT_MAX_METHODS);
1434 	     i < IB_MGMT_MAX_METHODS;
1435 	     i = find_next_bit(mad_reg_req->method_mask, IB_MGMT_MAX_METHODS,
1436 			       1+i)) {
1437 		(*method)->agent[i] = agent_priv;
1438 	}
1439 	return 0;
1440 
1441 error4:
1442 	/* Remove any methods for this mad agent */
1443 	remove_methods_mad_agent(*method, agent_priv);
1444 	/* Now, check to see if there are any methods in use */
1445 	if (!check_method_table(*method)) {
1446 		/* If not, release management method table */
1447 		kfree(*method);
1448 		*method = NULL;
1449 	}
1450 	ret = -EINVAL;
1451 error3:
1452 	if (vendor_class) {
1453 		(*vendor_table)->vendor_class[vclass] = NULL;
1454 		kfree(vendor_class);
1455 	}
1456 error2:
1457 	if (vendor) {
1458 		*vendor_table = NULL;
1459 		kfree(vendor);
1460 	}
1461 error1:
1462 	return ret;
1463 }
1464 
1465 static void remove_mad_reg_req(struct ib_mad_agent_private *agent_priv)
1466 {
1467 	struct ib_mad_port_private *port_priv;
1468 	struct ib_mad_mgmt_class_table *class;
1469 	struct ib_mad_mgmt_method_table *method;
1470 	struct ib_mad_mgmt_vendor_class_table *vendor;
1471 	struct ib_mad_mgmt_vendor_class *vendor_class;
1472 	int index;
1473 	u8 mgmt_class;
1474 
1475 	/*
1476 	 * Was MAD registration request supplied
1477 	 * with original registration ?
1478 	 */
1479 	if (!agent_priv->reg_req) {
1480 		goto out;
1481 	}
1482 
1483 	port_priv = agent_priv->qp_info->port_priv;
1484 	mgmt_class = convert_mgmt_class(agent_priv->reg_req->mgmt_class);
1485 	class = port_priv->version[
1486 			agent_priv->reg_req->mgmt_class_version].class;
1487 	if (!class)
1488 		goto vendor_check;
1489 
1490 	method = class->method_table[mgmt_class];
1491 	if (method) {
1492 		/* Remove any methods for this mad agent */
1493 		remove_methods_mad_agent(method, agent_priv);
1494 		/* Now, check to see if there are any methods still in use */
1495 		if (!check_method_table(method)) {
1496 			/* If not, release management method table */
1497 			 kfree(method);
1498 			 class->method_table[mgmt_class] = NULL;
1499 			 /* Any management classes left ? */
1500 			if (!check_class_table(class)) {
1501 				/* If not, release management class table */
1502 				kfree(class);
1503 				port_priv->version[
1504 					agent_priv->reg_req->
1505 					mgmt_class_version].class = NULL;
1506 			}
1507 		}
1508 	}
1509 
1510 vendor_check:
1511 	if (!is_vendor_class(mgmt_class))
1512 		goto out;
1513 
1514 	/* normalize mgmt_class to vendor range 2 */
1515 	mgmt_class = vendor_class_index(agent_priv->reg_req->mgmt_class);
1516 	vendor = port_priv->version[
1517 			agent_priv->reg_req->mgmt_class_version].vendor;
1518 
1519 	if (!vendor)
1520 		goto out;
1521 
1522 	vendor_class = vendor->vendor_class[mgmt_class];
1523 	if (vendor_class) {
1524 		index = find_vendor_oui(vendor_class, agent_priv->reg_req->oui);
1525 		if (index < 0)
1526 			goto out;
1527 		method = vendor_class->method_table[index];
1528 		if (method) {
1529 			/* Remove any methods for this mad agent */
1530 			remove_methods_mad_agent(method, agent_priv);
1531 			/*
1532 			 * Now, check to see if there are
1533 			 * any methods still in use
1534 			 */
1535 			if (!check_method_table(method)) {
1536 				/* If not, release management method table */
1537 				kfree(method);
1538 				vendor_class->method_table[index] = NULL;
1539 				memset(vendor_class->oui[index], 0, 3);
1540 				/* Any OUIs left ? */
1541 				if (!check_vendor_class(vendor_class)) {
1542 					/* If not, release vendor class table */
1543 					kfree(vendor_class);
1544 					vendor->vendor_class[mgmt_class] = NULL;
1545 					/* Any other vendor classes left ? */
1546 					if (!check_vendor_table(vendor)) {
1547 						kfree(vendor);
1548 						port_priv->version[
1549 							agent_priv->reg_req->
1550 							mgmt_class_version].
1551 							vendor = NULL;
1552 					}
1553 				}
1554 			}
1555 		}
1556 	}
1557 
1558 out:
1559 	return;
1560 }
1561 
1562 static struct ib_mad_agent_private *
1563 find_mad_agent(struct ib_mad_port_private *port_priv,
1564 	       struct ib_mad *mad)
1565 {
1566 	struct ib_mad_agent_private *mad_agent = NULL;
1567 	unsigned long flags;
1568 
1569 	spin_lock_irqsave(&port_priv->reg_lock, flags);
1570 	if (ib_response_mad(mad)) {
1571 		u32 hi_tid;
1572 		struct ib_mad_agent_private *entry;
1573 
1574 		/*
1575 		 * Routing is based on high 32 bits of transaction ID
1576 		 * of MAD.
1577 		 */
1578 		hi_tid = be64_to_cpu(mad->mad_hdr.tid) >> 32;
1579 		list_for_each_entry(entry, &port_priv->agent_list, agent_list) {
1580 			if (entry->agent.hi_tid == hi_tid) {
1581 				mad_agent = entry;
1582 				break;
1583 			}
1584 		}
1585 	} else {
1586 		struct ib_mad_mgmt_class_table *class;
1587 		struct ib_mad_mgmt_method_table *method;
1588 		struct ib_mad_mgmt_vendor_class_table *vendor;
1589 		struct ib_mad_mgmt_vendor_class *vendor_class;
1590 		struct ib_vendor_mad *vendor_mad;
1591 		int index;
1592 
1593 		/*
1594 		 * Routing is based on version, class, and method
1595 		 * For "newer" vendor MADs, also based on OUI
1596 		 */
1597 		if (mad->mad_hdr.class_version >= MAX_MGMT_VERSION)
1598 			goto out;
1599 		if (!is_vendor_class(mad->mad_hdr.mgmt_class)) {
1600 			class = port_priv->version[
1601 					mad->mad_hdr.class_version].class;
1602 			if (!class)
1603 				goto out;
1604 			method = class->method_table[convert_mgmt_class(
1605 							mad->mad_hdr.mgmt_class)];
1606 			if (method)
1607 				mad_agent = method->agent[mad->mad_hdr.method &
1608 							  ~IB_MGMT_METHOD_RESP];
1609 		} else {
1610 			vendor = port_priv->version[
1611 					mad->mad_hdr.class_version].vendor;
1612 			if (!vendor)
1613 				goto out;
1614 			vendor_class = vendor->vendor_class[vendor_class_index(
1615 						mad->mad_hdr.mgmt_class)];
1616 			if (!vendor_class)
1617 				goto out;
1618 			/* Find matching OUI */
1619 			vendor_mad = (struct ib_vendor_mad *)mad;
1620 			index = find_vendor_oui(vendor_class, vendor_mad->oui);
1621 			if (index == -1)
1622 				goto out;
1623 			method = vendor_class->method_table[index];
1624 			if (method) {
1625 				mad_agent = method->agent[mad->mad_hdr.method &
1626 							  ~IB_MGMT_METHOD_RESP];
1627 			}
1628 		}
1629 	}
1630 
1631 	if (mad_agent) {
1632 		if (mad_agent->agent.recv_handler)
1633 			atomic_inc(&mad_agent->refcount);
1634 		else {
1635 			printk(KERN_NOTICE PFX "No receive handler for client "
1636 			       "%p on port %d\n",
1637 			       &mad_agent->agent, port_priv->port_num);
1638 			mad_agent = NULL;
1639 		}
1640 	}
1641 out:
1642 	spin_unlock_irqrestore(&port_priv->reg_lock, flags);
1643 
1644 	return mad_agent;
1645 }
1646 
1647 static int validate_mad(struct ib_mad *mad, u32 qp_num)
1648 {
1649 	int valid = 0;
1650 
1651 	/* Make sure MAD base version is understood */
1652 	if (mad->mad_hdr.base_version != IB_MGMT_BASE_VERSION) {
1653 		printk(KERN_ERR PFX "MAD received with unsupported base "
1654 		       "version %d\n", mad->mad_hdr.base_version);
1655 		goto out;
1656 	}
1657 
1658 	/* Filter SMI packets sent to other than QP0 */
1659 	if ((mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_SUBN_LID_ROUTED) ||
1660 	    (mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE)) {
1661 		if (qp_num == 0)
1662 			valid = 1;
1663 	} else {
1664 		/* Filter GSI packets sent to QP0 */
1665 		if (qp_num != 0)
1666 			valid = 1;
1667 	}
1668 
1669 out:
1670 	return valid;
1671 }
1672 
1673 static int is_data_mad(struct ib_mad_agent_private *mad_agent_priv,
1674 		       struct ib_mad_hdr *mad_hdr)
1675 {
1676 	struct ib_rmpp_mad *rmpp_mad;
1677 
1678 	rmpp_mad = (struct ib_rmpp_mad *)mad_hdr;
1679 	return !mad_agent_priv->agent.rmpp_version ||
1680 		!(ib_get_rmpp_flags(&rmpp_mad->rmpp_hdr) &
1681 				    IB_MGMT_RMPP_FLAG_ACTIVE) ||
1682 		(rmpp_mad->rmpp_hdr.rmpp_type == IB_MGMT_RMPP_TYPE_DATA);
1683 }
1684 
1685 static inline int rcv_has_same_class(struct ib_mad_send_wr_private *wr,
1686 				     struct ib_mad_recv_wc *rwc)
1687 {
1688 	return ((struct ib_mad *)(wr->send_buf.mad))->mad_hdr.mgmt_class ==
1689 		rwc->recv_buf.mad->mad_hdr.mgmt_class;
1690 }
1691 
1692 static inline int rcv_has_same_gid(struct ib_mad_agent_private *mad_agent_priv,
1693 				   struct ib_mad_send_wr_private *wr,
1694 				   struct ib_mad_recv_wc *rwc )
1695 {
1696 	struct ib_ah_attr attr;
1697 	u8 send_resp, rcv_resp;
1698 	union ib_gid sgid;
1699 	struct ib_device *device = mad_agent_priv->agent.device;
1700 	u8 port_num = mad_agent_priv->agent.port_num;
1701 	u8 lmc;
1702 
1703 	send_resp = ib_response_mad((struct ib_mad *)wr->send_buf.mad);
1704 	rcv_resp = ib_response_mad(rwc->recv_buf.mad);
1705 
1706 	if (send_resp == rcv_resp)
1707 		/* both requests, or both responses. GIDs different */
1708 		return 0;
1709 
1710 	if (ib_query_ah(wr->send_buf.ah, &attr))
1711 		/* Assume not equal, to avoid false positives. */
1712 		return 0;
1713 
1714 	if (!!(attr.ah_flags & IB_AH_GRH) !=
1715 	    !!(rwc->wc->wc_flags & IB_WC_GRH))
1716 		/* one has GID, other does not.  Assume different */
1717 		return 0;
1718 
1719 	if (!send_resp && rcv_resp) {
1720 		/* is request/response. */
1721 		if (!(attr.ah_flags & IB_AH_GRH)) {
1722 			if (ib_get_cached_lmc(device, port_num, &lmc))
1723 				return 0;
1724 			return (!lmc || !((attr.src_path_bits ^
1725 					   rwc->wc->dlid_path_bits) &
1726 					  ((1 << lmc) - 1)));
1727 		} else {
1728 			if (ib_get_cached_gid(device, port_num,
1729 					      attr.grh.sgid_index, &sgid))
1730 				return 0;
1731 			return !memcmp(sgid.raw, rwc->recv_buf.grh->dgid.raw,
1732 				       16);
1733 		}
1734 	}
1735 
1736 	if (!(attr.ah_flags & IB_AH_GRH))
1737 		return attr.dlid == rwc->wc->slid;
1738 	else
1739 		return !memcmp(attr.grh.dgid.raw, rwc->recv_buf.grh->sgid.raw,
1740 			       16);
1741 }
1742 
1743 static inline int is_direct(u8 class)
1744 {
1745 	return (class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE);
1746 }
1747 
1748 struct ib_mad_send_wr_private*
1749 ib_find_send_mad(struct ib_mad_agent_private *mad_agent_priv,
1750 		 struct ib_mad_recv_wc *wc)
1751 {
1752 	struct ib_mad_send_wr_private *wr;
1753 	struct ib_mad *mad;
1754 
1755 	mad = (struct ib_mad *)wc->recv_buf.mad;
1756 
1757 	list_for_each_entry(wr, &mad_agent_priv->wait_list, agent_list) {
1758 		if ((wr->tid == mad->mad_hdr.tid) &&
1759 		    rcv_has_same_class(wr, wc) &&
1760 		    /*
1761 		     * Don't check GID for direct routed MADs.
1762 		     * These might have permissive LIDs.
1763 		     */
1764 		    (is_direct(wc->recv_buf.mad->mad_hdr.mgmt_class) ||
1765 		     rcv_has_same_gid(mad_agent_priv, wr, wc)))
1766 			return (wr->status == IB_WC_SUCCESS) ? wr : NULL;
1767 	}
1768 
1769 	/*
1770 	 * It's possible to receive the response before we've
1771 	 * been notified that the send has completed
1772 	 */
1773 	list_for_each_entry(wr, &mad_agent_priv->send_list, agent_list) {
1774 		if (is_data_mad(mad_agent_priv, wr->send_buf.mad) &&
1775 		    wr->tid == mad->mad_hdr.tid &&
1776 		    wr->timeout &&
1777 		    rcv_has_same_class(wr, wc) &&
1778 		    /*
1779 		     * Don't check GID for direct routed MADs.
1780 		     * These might have permissive LIDs.
1781 		     */
1782 		    (is_direct(wc->recv_buf.mad->mad_hdr.mgmt_class) ||
1783 		     rcv_has_same_gid(mad_agent_priv, wr, wc)))
1784 			/* Verify request has not been canceled */
1785 			return (wr->status == IB_WC_SUCCESS) ? wr : NULL;
1786 	}
1787 	return NULL;
1788 }
1789 
1790 void ib_mark_mad_done(struct ib_mad_send_wr_private *mad_send_wr)
1791 {
1792 	mad_send_wr->timeout = 0;
1793 	if (mad_send_wr->refcount == 1)
1794 		list_move_tail(&mad_send_wr->agent_list,
1795 			      &mad_send_wr->mad_agent_priv->done_list);
1796 }
1797 
1798 static void ib_mad_complete_recv(struct ib_mad_agent_private *mad_agent_priv,
1799 				 struct ib_mad_recv_wc *mad_recv_wc)
1800 {
1801 	struct ib_mad_send_wr_private *mad_send_wr;
1802 	struct ib_mad_send_wc mad_send_wc;
1803 	unsigned long flags;
1804 
1805 	INIT_LIST_HEAD(&mad_recv_wc->rmpp_list);
1806 	list_add(&mad_recv_wc->recv_buf.list, &mad_recv_wc->rmpp_list);
1807 	if (mad_agent_priv->agent.rmpp_version) {
1808 		mad_recv_wc = ib_process_rmpp_recv_wc(mad_agent_priv,
1809 						      mad_recv_wc);
1810 		if (!mad_recv_wc) {
1811 			deref_mad_agent(mad_agent_priv);
1812 			return;
1813 		}
1814 	}
1815 
1816 	/* Complete corresponding request */
1817 	if (ib_response_mad(mad_recv_wc->recv_buf.mad)) {
1818 		spin_lock_irqsave(&mad_agent_priv->lock, flags);
1819 		mad_send_wr = ib_find_send_mad(mad_agent_priv, mad_recv_wc);
1820 		if (!mad_send_wr) {
1821 			spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
1822 			ib_free_recv_mad(mad_recv_wc);
1823 			deref_mad_agent(mad_agent_priv);
1824 			return;
1825 		}
1826 		ib_mark_mad_done(mad_send_wr);
1827 		spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
1828 
1829 		/* Defined behavior is to complete response before request */
1830 		mad_recv_wc->wc->wr_id = (unsigned long) &mad_send_wr->send_buf;
1831 		mad_agent_priv->agent.recv_handler(&mad_agent_priv->agent,
1832 						   mad_recv_wc);
1833 		atomic_dec(&mad_agent_priv->refcount);
1834 
1835 		mad_send_wc.status = IB_WC_SUCCESS;
1836 		mad_send_wc.vendor_err = 0;
1837 		mad_send_wc.send_buf = &mad_send_wr->send_buf;
1838 		ib_mad_complete_send_wr(mad_send_wr, &mad_send_wc);
1839 	} else {
1840 		mad_agent_priv->agent.recv_handler(&mad_agent_priv->agent,
1841 						   mad_recv_wc);
1842 		deref_mad_agent(mad_agent_priv);
1843 	}
1844 }
1845 
1846 static void ib_mad_recv_done_handler(struct ib_mad_port_private *port_priv,
1847 				     struct ib_wc *wc)
1848 {
1849 	struct ib_mad_qp_info *qp_info;
1850 	struct ib_mad_private_header *mad_priv_hdr;
1851 	struct ib_mad_private *recv, *response = NULL;
1852 	struct ib_mad_list_head *mad_list;
1853 	struct ib_mad_agent_private *mad_agent;
1854 	int port_num;
1855 
1856 	mad_list = (struct ib_mad_list_head *)(unsigned long)wc->wr_id;
1857 	qp_info = mad_list->mad_queue->qp_info;
1858 	dequeue_mad(mad_list);
1859 
1860 	mad_priv_hdr = container_of(mad_list, struct ib_mad_private_header,
1861 				    mad_list);
1862 	recv = container_of(mad_priv_hdr, struct ib_mad_private, header);
1863 	ib_dma_unmap_single(port_priv->device,
1864 			    recv->header.mapping,
1865 			    sizeof(struct ib_mad_private) -
1866 			      sizeof(struct ib_mad_private_header),
1867 			    DMA_FROM_DEVICE);
1868 
1869 	/* Setup MAD receive work completion from "normal" work completion */
1870 	recv->header.wc = *wc;
1871 	recv->header.recv_wc.wc = &recv->header.wc;
1872 	recv->header.recv_wc.mad_len = sizeof(struct ib_mad);
1873 	recv->header.recv_wc.recv_buf.mad = &recv->mad.mad;
1874 	recv->header.recv_wc.recv_buf.grh = &recv->grh;
1875 
1876 	if (atomic_read(&qp_info->snoop_count))
1877 		snoop_recv(qp_info, &recv->header.recv_wc, IB_MAD_SNOOP_RECVS);
1878 
1879 	/* Validate MAD */
1880 	if (!validate_mad(&recv->mad.mad, qp_info->qp->qp_num))
1881 		goto out;
1882 
1883 	response = kmem_cache_alloc(ib_mad_cache, GFP_KERNEL);
1884 	if (!response) {
1885 		printk(KERN_ERR PFX "ib_mad_recv_done_handler no memory "
1886 		       "for response buffer\n");
1887 		goto out;
1888 	}
1889 
1890 	if (port_priv->device->node_type == RDMA_NODE_IB_SWITCH)
1891 		port_num = wc->port_num;
1892 	else
1893 		port_num = port_priv->port_num;
1894 
1895 	if (recv->mad.mad.mad_hdr.mgmt_class ==
1896 	    IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE) {
1897 		enum smi_forward_action retsmi;
1898 
1899 		if (smi_handle_dr_smp_recv(&recv->mad.smp,
1900 					   port_priv->device->node_type,
1901 					   port_num,
1902 					   port_priv->device->phys_port_cnt) ==
1903 					   IB_SMI_DISCARD)
1904 			goto out;
1905 
1906 		retsmi = smi_check_forward_dr_smp(&recv->mad.smp);
1907 		if (retsmi == IB_SMI_LOCAL)
1908 			goto local;
1909 
1910 		if (retsmi == IB_SMI_SEND) { /* don't forward */
1911 			if (smi_handle_dr_smp_send(&recv->mad.smp,
1912 						   port_priv->device->node_type,
1913 						   port_num) == IB_SMI_DISCARD)
1914 				goto out;
1915 
1916 			if (smi_check_local_smp(&recv->mad.smp, port_priv->device) == IB_SMI_DISCARD)
1917 				goto out;
1918 		} else if (port_priv->device->node_type == RDMA_NODE_IB_SWITCH) {
1919 			/* forward case for switches */
1920 			memcpy(response, recv, sizeof(*response));
1921 			response->header.recv_wc.wc = &response->header.wc;
1922 			response->header.recv_wc.recv_buf.mad = &response->mad.mad;
1923 			response->header.recv_wc.recv_buf.grh = &response->grh;
1924 
1925 			agent_send_response(&response->mad.mad,
1926 					    &response->grh, wc,
1927 					    port_priv->device,
1928 					    smi_get_fwd_port(&recv->mad.smp),
1929 					    qp_info->qp->qp_num);
1930 
1931 			goto out;
1932 		}
1933 	}
1934 
1935 local:
1936 	/* Give driver "right of first refusal" on incoming MAD */
1937 	if (port_priv->device->process_mad) {
1938 		int ret;
1939 
1940 		ret = port_priv->device->process_mad(port_priv->device, 0,
1941 						     port_priv->port_num,
1942 						     wc, &recv->grh,
1943 						     &recv->mad.mad,
1944 						     &response->mad.mad);
1945 		if (ret & IB_MAD_RESULT_SUCCESS) {
1946 			if (ret & IB_MAD_RESULT_CONSUMED)
1947 				goto out;
1948 			if (ret & IB_MAD_RESULT_REPLY) {
1949 				agent_send_response(&response->mad.mad,
1950 						    &recv->grh, wc,
1951 						    port_priv->device,
1952 						    port_num,
1953 						    qp_info->qp->qp_num);
1954 				goto out;
1955 			}
1956 		}
1957 	}
1958 
1959 	mad_agent = find_mad_agent(port_priv, &recv->mad.mad);
1960 	if (mad_agent) {
1961 		ib_mad_complete_recv(mad_agent, &recv->header.recv_wc);
1962 		/*
1963 		 * recv is freed up in error cases in ib_mad_complete_recv
1964 		 * or via recv_handler in ib_mad_complete_recv()
1965 		 */
1966 		recv = NULL;
1967 	}
1968 
1969 out:
1970 	/* Post another receive request for this QP */
1971 	if (response) {
1972 		ib_mad_post_receive_mads(qp_info, response);
1973 		if (recv)
1974 			kmem_cache_free(ib_mad_cache, recv);
1975 	} else
1976 		ib_mad_post_receive_mads(qp_info, recv);
1977 }
1978 
1979 static void adjust_timeout(struct ib_mad_agent_private *mad_agent_priv)
1980 {
1981 	struct ib_mad_send_wr_private *mad_send_wr;
1982 	unsigned long delay;
1983 
1984 	if (list_empty(&mad_agent_priv->wait_list)) {
1985 		__cancel_delayed_work(&mad_agent_priv->timed_work);
1986 	} else {
1987 		mad_send_wr = list_entry(mad_agent_priv->wait_list.next,
1988 					 struct ib_mad_send_wr_private,
1989 					 agent_list);
1990 
1991 		if (time_after(mad_agent_priv->timeout,
1992 			       mad_send_wr->timeout)) {
1993 			mad_agent_priv->timeout = mad_send_wr->timeout;
1994 			__cancel_delayed_work(&mad_agent_priv->timed_work);
1995 			delay = mad_send_wr->timeout - jiffies;
1996 			if ((long)delay <= 0)
1997 				delay = 1;
1998 			queue_delayed_work(mad_agent_priv->qp_info->
1999 					   port_priv->wq,
2000 					   &mad_agent_priv->timed_work, delay);
2001 		}
2002 	}
2003 }
2004 
2005 static void wait_for_response(struct ib_mad_send_wr_private *mad_send_wr)
2006 {
2007 	struct ib_mad_agent_private *mad_agent_priv;
2008 	struct ib_mad_send_wr_private *temp_mad_send_wr;
2009 	struct list_head *list_item;
2010 	unsigned long delay;
2011 
2012 	mad_agent_priv = mad_send_wr->mad_agent_priv;
2013 	list_del(&mad_send_wr->agent_list);
2014 
2015 	delay = mad_send_wr->timeout;
2016 	mad_send_wr->timeout += jiffies;
2017 
2018 	if (delay) {
2019 		list_for_each_prev(list_item, &mad_agent_priv->wait_list) {
2020 			temp_mad_send_wr = list_entry(list_item,
2021 						struct ib_mad_send_wr_private,
2022 						agent_list);
2023 			if (time_after(mad_send_wr->timeout,
2024 				       temp_mad_send_wr->timeout))
2025 				break;
2026 		}
2027 	}
2028 	else
2029 		list_item = &mad_agent_priv->wait_list;
2030 	list_add(&mad_send_wr->agent_list, list_item);
2031 
2032 	/* Reschedule a work item if we have a shorter timeout */
2033 	if (mad_agent_priv->wait_list.next == &mad_send_wr->agent_list) {
2034 		__cancel_delayed_work(&mad_agent_priv->timed_work);
2035 		queue_delayed_work(mad_agent_priv->qp_info->port_priv->wq,
2036 				   &mad_agent_priv->timed_work, delay);
2037 	}
2038 }
2039 
2040 void ib_reset_mad_timeout(struct ib_mad_send_wr_private *mad_send_wr,
2041 			  int timeout_ms)
2042 {
2043 	mad_send_wr->timeout = msecs_to_jiffies(timeout_ms);
2044 	wait_for_response(mad_send_wr);
2045 }
2046 
2047 /*
2048  * Process a send work completion
2049  */
2050 void ib_mad_complete_send_wr(struct ib_mad_send_wr_private *mad_send_wr,
2051 			     struct ib_mad_send_wc *mad_send_wc)
2052 {
2053 	struct ib_mad_agent_private	*mad_agent_priv;
2054 	unsigned long			flags;
2055 	int				ret;
2056 
2057 	mad_agent_priv = mad_send_wr->mad_agent_priv;
2058 	spin_lock_irqsave(&mad_agent_priv->lock, flags);
2059 	if (mad_agent_priv->agent.rmpp_version) {
2060 		ret = ib_process_rmpp_send_wc(mad_send_wr, mad_send_wc);
2061 		if (ret == IB_RMPP_RESULT_CONSUMED)
2062 			goto done;
2063 	} else
2064 		ret = IB_RMPP_RESULT_UNHANDLED;
2065 
2066 	if (mad_send_wc->status != IB_WC_SUCCESS &&
2067 	    mad_send_wr->status == IB_WC_SUCCESS) {
2068 		mad_send_wr->status = mad_send_wc->status;
2069 		mad_send_wr->refcount -= (mad_send_wr->timeout > 0);
2070 	}
2071 
2072 	if (--mad_send_wr->refcount > 0) {
2073 		if (mad_send_wr->refcount == 1 && mad_send_wr->timeout &&
2074 		    mad_send_wr->status == IB_WC_SUCCESS) {
2075 			wait_for_response(mad_send_wr);
2076 		}
2077 		goto done;
2078 	}
2079 
2080 	/* Remove send from MAD agent and notify client of completion */
2081 	list_del(&mad_send_wr->agent_list);
2082 	adjust_timeout(mad_agent_priv);
2083 	spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
2084 
2085 	if (mad_send_wr->status != IB_WC_SUCCESS )
2086 		mad_send_wc->status = mad_send_wr->status;
2087 	if (ret == IB_RMPP_RESULT_INTERNAL)
2088 		ib_rmpp_send_handler(mad_send_wc);
2089 	else
2090 		mad_agent_priv->agent.send_handler(&mad_agent_priv->agent,
2091 						   mad_send_wc);
2092 
2093 	/* Release reference on agent taken when sending */
2094 	deref_mad_agent(mad_agent_priv);
2095 	return;
2096 done:
2097 	spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
2098 }
2099 
2100 static void ib_mad_send_done_handler(struct ib_mad_port_private *port_priv,
2101 				     struct ib_wc *wc)
2102 {
2103 	struct ib_mad_send_wr_private	*mad_send_wr, *queued_send_wr;
2104 	struct ib_mad_list_head		*mad_list;
2105 	struct ib_mad_qp_info		*qp_info;
2106 	struct ib_mad_queue		*send_queue;
2107 	struct ib_send_wr		*bad_send_wr;
2108 	struct ib_mad_send_wc		mad_send_wc;
2109 	unsigned long flags;
2110 	int ret;
2111 
2112 	mad_list = (struct ib_mad_list_head *)(unsigned long)wc->wr_id;
2113 	mad_send_wr = container_of(mad_list, struct ib_mad_send_wr_private,
2114 				   mad_list);
2115 	send_queue = mad_list->mad_queue;
2116 	qp_info = send_queue->qp_info;
2117 
2118 retry:
2119 	ib_dma_unmap_single(mad_send_wr->send_buf.mad_agent->device,
2120 			    mad_send_wr->header_mapping,
2121 			    mad_send_wr->sg_list[0].length, DMA_TO_DEVICE);
2122 	ib_dma_unmap_single(mad_send_wr->send_buf.mad_agent->device,
2123 			    mad_send_wr->payload_mapping,
2124 			    mad_send_wr->sg_list[1].length, DMA_TO_DEVICE);
2125 	queued_send_wr = NULL;
2126 	spin_lock_irqsave(&send_queue->lock, flags);
2127 	list_del(&mad_list->list);
2128 
2129 	/* Move queued send to the send queue */
2130 	if (send_queue->count-- > send_queue->max_active) {
2131 		mad_list = container_of(qp_info->overflow_list.next,
2132 					struct ib_mad_list_head, list);
2133 		queued_send_wr = container_of(mad_list,
2134 					struct ib_mad_send_wr_private,
2135 					mad_list);
2136 		list_move_tail(&mad_list->list, &send_queue->list);
2137 	}
2138 	spin_unlock_irqrestore(&send_queue->lock, flags);
2139 
2140 	mad_send_wc.send_buf = &mad_send_wr->send_buf;
2141 	mad_send_wc.status = wc->status;
2142 	mad_send_wc.vendor_err = wc->vendor_err;
2143 	if (atomic_read(&qp_info->snoop_count))
2144 		snoop_send(qp_info, &mad_send_wr->send_buf, &mad_send_wc,
2145 			   IB_MAD_SNOOP_SEND_COMPLETIONS);
2146 	ib_mad_complete_send_wr(mad_send_wr, &mad_send_wc);
2147 
2148 	if (queued_send_wr) {
2149 		ret = ib_post_send(qp_info->qp, &queued_send_wr->send_wr,
2150 				   &bad_send_wr);
2151 		if (ret) {
2152 			printk(KERN_ERR PFX "ib_post_send failed: %d\n", ret);
2153 			mad_send_wr = queued_send_wr;
2154 			wc->status = IB_WC_LOC_QP_OP_ERR;
2155 			goto retry;
2156 		}
2157 	}
2158 }
2159 
2160 static void mark_sends_for_retry(struct ib_mad_qp_info *qp_info)
2161 {
2162 	struct ib_mad_send_wr_private *mad_send_wr;
2163 	struct ib_mad_list_head *mad_list;
2164 	unsigned long flags;
2165 
2166 	spin_lock_irqsave(&qp_info->send_queue.lock, flags);
2167 	list_for_each_entry(mad_list, &qp_info->send_queue.list, list) {
2168 		mad_send_wr = container_of(mad_list,
2169 					   struct ib_mad_send_wr_private,
2170 					   mad_list);
2171 		mad_send_wr->retry = 1;
2172 	}
2173 	spin_unlock_irqrestore(&qp_info->send_queue.lock, flags);
2174 }
2175 
2176 static void mad_error_handler(struct ib_mad_port_private *port_priv,
2177 			      struct ib_wc *wc)
2178 {
2179 	struct ib_mad_list_head *mad_list;
2180 	struct ib_mad_qp_info *qp_info;
2181 	struct ib_mad_send_wr_private *mad_send_wr;
2182 	int ret;
2183 
2184 	/* Determine if failure was a send or receive */
2185 	mad_list = (struct ib_mad_list_head *)(unsigned long)wc->wr_id;
2186 	qp_info = mad_list->mad_queue->qp_info;
2187 	if (mad_list->mad_queue == &qp_info->recv_queue)
2188 		/*
2189 		 * Receive errors indicate that the QP has entered the error
2190 		 * state - error handling/shutdown code will cleanup
2191 		 */
2192 		return;
2193 
2194 	/*
2195 	 * Send errors will transition the QP to SQE - move
2196 	 * QP to RTS and repost flushed work requests
2197 	 */
2198 	mad_send_wr = container_of(mad_list, struct ib_mad_send_wr_private,
2199 				   mad_list);
2200 	if (wc->status == IB_WC_WR_FLUSH_ERR) {
2201 		if (mad_send_wr->retry) {
2202 			/* Repost send */
2203 			struct ib_send_wr *bad_send_wr;
2204 
2205 			mad_send_wr->retry = 0;
2206 			ret = ib_post_send(qp_info->qp, &mad_send_wr->send_wr,
2207 					&bad_send_wr);
2208 			if (ret)
2209 				ib_mad_send_done_handler(port_priv, wc);
2210 		} else
2211 			ib_mad_send_done_handler(port_priv, wc);
2212 	} else {
2213 		struct ib_qp_attr *attr;
2214 
2215 		/* Transition QP to RTS and fail offending send */
2216 		attr = kmalloc(sizeof *attr, GFP_KERNEL);
2217 		if (attr) {
2218 			attr->qp_state = IB_QPS_RTS;
2219 			attr->cur_qp_state = IB_QPS_SQE;
2220 			ret = ib_modify_qp(qp_info->qp, attr,
2221 					   IB_QP_STATE | IB_QP_CUR_STATE);
2222 			kfree(attr);
2223 			if (ret)
2224 				printk(KERN_ERR PFX "mad_error_handler - "
2225 				       "ib_modify_qp to RTS : %d\n", ret);
2226 			else
2227 				mark_sends_for_retry(qp_info);
2228 		}
2229 		ib_mad_send_done_handler(port_priv, wc);
2230 	}
2231 }
2232 
2233 /*
2234  * IB MAD completion callback
2235  */
2236 static void ib_mad_completion_handler(struct work_struct *work)
2237 {
2238 	struct ib_mad_port_private *port_priv;
2239 	struct ib_wc wc;
2240 
2241 	port_priv = container_of(work, struct ib_mad_port_private, work);
2242 	ib_req_notify_cq(port_priv->cq, IB_CQ_NEXT_COMP);
2243 
2244 	while (ib_poll_cq(port_priv->cq, 1, &wc) == 1) {
2245 		if (wc.status == IB_WC_SUCCESS) {
2246 			switch (wc.opcode) {
2247 			case IB_WC_SEND:
2248 				ib_mad_send_done_handler(port_priv, &wc);
2249 				break;
2250 			case IB_WC_RECV:
2251 				ib_mad_recv_done_handler(port_priv, &wc);
2252 				break;
2253 			default:
2254 				BUG_ON(1);
2255 				break;
2256 			}
2257 		} else
2258 			mad_error_handler(port_priv, &wc);
2259 	}
2260 }
2261 
2262 static void cancel_mads(struct ib_mad_agent_private *mad_agent_priv)
2263 {
2264 	unsigned long flags;
2265 	struct ib_mad_send_wr_private *mad_send_wr, *temp_mad_send_wr;
2266 	struct ib_mad_send_wc mad_send_wc;
2267 	struct list_head cancel_list;
2268 
2269 	INIT_LIST_HEAD(&cancel_list);
2270 
2271 	spin_lock_irqsave(&mad_agent_priv->lock, flags);
2272 	list_for_each_entry_safe(mad_send_wr, temp_mad_send_wr,
2273 				 &mad_agent_priv->send_list, agent_list) {
2274 		if (mad_send_wr->status == IB_WC_SUCCESS) {
2275 			mad_send_wr->status = IB_WC_WR_FLUSH_ERR;
2276 			mad_send_wr->refcount -= (mad_send_wr->timeout > 0);
2277 		}
2278 	}
2279 
2280 	/* Empty wait list to prevent receives from finding a request */
2281 	list_splice_init(&mad_agent_priv->wait_list, &cancel_list);
2282 	spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
2283 
2284 	/* Report all cancelled requests */
2285 	mad_send_wc.status = IB_WC_WR_FLUSH_ERR;
2286 	mad_send_wc.vendor_err = 0;
2287 
2288 	list_for_each_entry_safe(mad_send_wr, temp_mad_send_wr,
2289 				 &cancel_list, agent_list) {
2290 		mad_send_wc.send_buf = &mad_send_wr->send_buf;
2291 		list_del(&mad_send_wr->agent_list);
2292 		mad_agent_priv->agent.send_handler(&mad_agent_priv->agent,
2293 						   &mad_send_wc);
2294 		atomic_dec(&mad_agent_priv->refcount);
2295 	}
2296 }
2297 
2298 static struct ib_mad_send_wr_private*
2299 find_send_wr(struct ib_mad_agent_private *mad_agent_priv,
2300 	     struct ib_mad_send_buf *send_buf)
2301 {
2302 	struct ib_mad_send_wr_private *mad_send_wr;
2303 
2304 	list_for_each_entry(mad_send_wr, &mad_agent_priv->wait_list,
2305 			    agent_list) {
2306 		if (&mad_send_wr->send_buf == send_buf)
2307 			return mad_send_wr;
2308 	}
2309 
2310 	list_for_each_entry(mad_send_wr, &mad_agent_priv->send_list,
2311 			    agent_list) {
2312 		if (is_data_mad(mad_agent_priv, mad_send_wr->send_buf.mad) &&
2313 		    &mad_send_wr->send_buf == send_buf)
2314 			return mad_send_wr;
2315 	}
2316 	return NULL;
2317 }
2318 
2319 int ib_modify_mad(struct ib_mad_agent *mad_agent,
2320 		  struct ib_mad_send_buf *send_buf, u32 timeout_ms)
2321 {
2322 	struct ib_mad_agent_private *mad_agent_priv;
2323 	struct ib_mad_send_wr_private *mad_send_wr;
2324 	unsigned long flags;
2325 	int active;
2326 
2327 	mad_agent_priv = container_of(mad_agent, struct ib_mad_agent_private,
2328 				      agent);
2329 	spin_lock_irqsave(&mad_agent_priv->lock, flags);
2330 	mad_send_wr = find_send_wr(mad_agent_priv, send_buf);
2331 	if (!mad_send_wr || mad_send_wr->status != IB_WC_SUCCESS) {
2332 		spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
2333 		return -EINVAL;
2334 	}
2335 
2336 	active = (!mad_send_wr->timeout || mad_send_wr->refcount > 1);
2337 	if (!timeout_ms) {
2338 		mad_send_wr->status = IB_WC_WR_FLUSH_ERR;
2339 		mad_send_wr->refcount -= (mad_send_wr->timeout > 0);
2340 	}
2341 
2342 	mad_send_wr->send_buf.timeout_ms = timeout_ms;
2343 	if (active)
2344 		mad_send_wr->timeout = msecs_to_jiffies(timeout_ms);
2345 	else
2346 		ib_reset_mad_timeout(mad_send_wr, timeout_ms);
2347 
2348 	spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
2349 	return 0;
2350 }
2351 EXPORT_SYMBOL(ib_modify_mad);
2352 
2353 void ib_cancel_mad(struct ib_mad_agent *mad_agent,
2354 		   struct ib_mad_send_buf *send_buf)
2355 {
2356 	ib_modify_mad(mad_agent, send_buf, 0);
2357 }
2358 EXPORT_SYMBOL(ib_cancel_mad);
2359 
2360 static void local_completions(struct work_struct *work)
2361 {
2362 	struct ib_mad_agent_private *mad_agent_priv;
2363 	struct ib_mad_local_private *local;
2364 	struct ib_mad_agent_private *recv_mad_agent;
2365 	unsigned long flags;
2366 	int free_mad;
2367 	struct ib_wc wc;
2368 	struct ib_mad_send_wc mad_send_wc;
2369 
2370 	mad_agent_priv =
2371 		container_of(work, struct ib_mad_agent_private, local_work);
2372 
2373 	spin_lock_irqsave(&mad_agent_priv->lock, flags);
2374 	while (!list_empty(&mad_agent_priv->local_list)) {
2375 		local = list_entry(mad_agent_priv->local_list.next,
2376 				   struct ib_mad_local_private,
2377 				   completion_list);
2378 		list_del(&local->completion_list);
2379 		spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
2380 		free_mad = 0;
2381 		if (local->mad_priv) {
2382 			recv_mad_agent = local->recv_mad_agent;
2383 			if (!recv_mad_agent) {
2384 				printk(KERN_ERR PFX "No receive MAD agent for local completion\n");
2385 				free_mad = 1;
2386 				goto local_send_completion;
2387 			}
2388 
2389 			/*
2390 			 * Defined behavior is to complete response
2391 			 * before request
2392 			 */
2393 			build_smp_wc(recv_mad_agent->agent.qp,
2394 				     (unsigned long) local->mad_send_wr,
2395 				     be16_to_cpu(IB_LID_PERMISSIVE),
2396 				     0, recv_mad_agent->agent.port_num, &wc);
2397 
2398 			local->mad_priv->header.recv_wc.wc = &wc;
2399 			local->mad_priv->header.recv_wc.mad_len =
2400 						sizeof(struct ib_mad);
2401 			INIT_LIST_HEAD(&local->mad_priv->header.recv_wc.rmpp_list);
2402 			list_add(&local->mad_priv->header.recv_wc.recv_buf.list,
2403 				 &local->mad_priv->header.recv_wc.rmpp_list);
2404 			local->mad_priv->header.recv_wc.recv_buf.grh = NULL;
2405 			local->mad_priv->header.recv_wc.recv_buf.mad =
2406 						&local->mad_priv->mad.mad;
2407 			if (atomic_read(&recv_mad_agent->qp_info->snoop_count))
2408 				snoop_recv(recv_mad_agent->qp_info,
2409 					  &local->mad_priv->header.recv_wc,
2410 					   IB_MAD_SNOOP_RECVS);
2411 			recv_mad_agent->agent.recv_handler(
2412 						&recv_mad_agent->agent,
2413 						&local->mad_priv->header.recv_wc);
2414 			spin_lock_irqsave(&recv_mad_agent->lock, flags);
2415 			atomic_dec(&recv_mad_agent->refcount);
2416 			spin_unlock_irqrestore(&recv_mad_agent->lock, flags);
2417 		}
2418 
2419 local_send_completion:
2420 		/* Complete send */
2421 		mad_send_wc.status = IB_WC_SUCCESS;
2422 		mad_send_wc.vendor_err = 0;
2423 		mad_send_wc.send_buf = &local->mad_send_wr->send_buf;
2424 		if (atomic_read(&mad_agent_priv->qp_info->snoop_count))
2425 			snoop_send(mad_agent_priv->qp_info,
2426 				   &local->mad_send_wr->send_buf,
2427 				   &mad_send_wc, IB_MAD_SNOOP_SEND_COMPLETIONS);
2428 		mad_agent_priv->agent.send_handler(&mad_agent_priv->agent,
2429 						   &mad_send_wc);
2430 
2431 		spin_lock_irqsave(&mad_agent_priv->lock, flags);
2432 		atomic_dec(&mad_agent_priv->refcount);
2433 		if (free_mad)
2434 			kmem_cache_free(ib_mad_cache, local->mad_priv);
2435 		kfree(local);
2436 	}
2437 	spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
2438 }
2439 
2440 static int retry_send(struct ib_mad_send_wr_private *mad_send_wr)
2441 {
2442 	int ret;
2443 
2444 	if (!mad_send_wr->retries_left)
2445 		return -ETIMEDOUT;
2446 
2447 	mad_send_wr->retries_left--;
2448 	mad_send_wr->send_buf.retries++;
2449 
2450 	mad_send_wr->timeout = msecs_to_jiffies(mad_send_wr->send_buf.timeout_ms);
2451 
2452 	if (mad_send_wr->mad_agent_priv->agent.rmpp_version) {
2453 		ret = ib_retry_rmpp(mad_send_wr);
2454 		switch (ret) {
2455 		case IB_RMPP_RESULT_UNHANDLED:
2456 			ret = ib_send_mad(mad_send_wr);
2457 			break;
2458 		case IB_RMPP_RESULT_CONSUMED:
2459 			ret = 0;
2460 			break;
2461 		default:
2462 			ret = -ECOMM;
2463 			break;
2464 		}
2465 	} else
2466 		ret = ib_send_mad(mad_send_wr);
2467 
2468 	if (!ret) {
2469 		mad_send_wr->refcount++;
2470 		list_add_tail(&mad_send_wr->agent_list,
2471 			      &mad_send_wr->mad_agent_priv->send_list);
2472 	}
2473 	return ret;
2474 }
2475 
2476 static void timeout_sends(struct work_struct *work)
2477 {
2478 	struct ib_mad_agent_private *mad_agent_priv;
2479 	struct ib_mad_send_wr_private *mad_send_wr;
2480 	struct ib_mad_send_wc mad_send_wc;
2481 	unsigned long flags, delay;
2482 
2483 	mad_agent_priv = container_of(work, struct ib_mad_agent_private,
2484 				      timed_work.work);
2485 	mad_send_wc.vendor_err = 0;
2486 
2487 	spin_lock_irqsave(&mad_agent_priv->lock, flags);
2488 	while (!list_empty(&mad_agent_priv->wait_list)) {
2489 		mad_send_wr = list_entry(mad_agent_priv->wait_list.next,
2490 					 struct ib_mad_send_wr_private,
2491 					 agent_list);
2492 
2493 		if (time_after(mad_send_wr->timeout, jiffies)) {
2494 			delay = mad_send_wr->timeout - jiffies;
2495 			if ((long)delay <= 0)
2496 				delay = 1;
2497 			queue_delayed_work(mad_agent_priv->qp_info->
2498 					   port_priv->wq,
2499 					   &mad_agent_priv->timed_work, delay);
2500 			break;
2501 		}
2502 
2503 		list_del(&mad_send_wr->agent_list);
2504 		if (mad_send_wr->status == IB_WC_SUCCESS &&
2505 		    !retry_send(mad_send_wr))
2506 			continue;
2507 
2508 		spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
2509 
2510 		if (mad_send_wr->status == IB_WC_SUCCESS)
2511 			mad_send_wc.status = IB_WC_RESP_TIMEOUT_ERR;
2512 		else
2513 			mad_send_wc.status = mad_send_wr->status;
2514 		mad_send_wc.send_buf = &mad_send_wr->send_buf;
2515 		mad_agent_priv->agent.send_handler(&mad_agent_priv->agent,
2516 						   &mad_send_wc);
2517 
2518 		atomic_dec(&mad_agent_priv->refcount);
2519 		spin_lock_irqsave(&mad_agent_priv->lock, flags);
2520 	}
2521 	spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
2522 }
2523 
2524 static void ib_mad_thread_completion_handler(struct ib_cq *cq, void *arg)
2525 {
2526 	struct ib_mad_port_private *port_priv = cq->cq_context;
2527 	unsigned long flags;
2528 
2529 	spin_lock_irqsave(&ib_mad_port_list_lock, flags);
2530 	if (!list_empty(&port_priv->port_list))
2531 		queue_work(port_priv->wq, &port_priv->work);
2532 	spin_unlock_irqrestore(&ib_mad_port_list_lock, flags);
2533 }
2534 
2535 /*
2536  * Allocate receive MADs and post receive WRs for them
2537  */
2538 static int ib_mad_post_receive_mads(struct ib_mad_qp_info *qp_info,
2539 				    struct ib_mad_private *mad)
2540 {
2541 	unsigned long flags;
2542 	int post, ret;
2543 	struct ib_mad_private *mad_priv;
2544 	struct ib_sge sg_list;
2545 	struct ib_recv_wr recv_wr, *bad_recv_wr;
2546 	struct ib_mad_queue *recv_queue = &qp_info->recv_queue;
2547 
2548 	/* Initialize common scatter list fields */
2549 	sg_list.length = sizeof *mad_priv - sizeof mad_priv->header;
2550 	sg_list.lkey = (*qp_info->port_priv->mr).lkey;
2551 
2552 	/* Initialize common receive WR fields */
2553 	recv_wr.next = NULL;
2554 	recv_wr.sg_list = &sg_list;
2555 	recv_wr.num_sge = 1;
2556 
2557 	do {
2558 		/* Allocate and map receive buffer */
2559 		if (mad) {
2560 			mad_priv = mad;
2561 			mad = NULL;
2562 		} else {
2563 			mad_priv = kmem_cache_alloc(ib_mad_cache, GFP_KERNEL);
2564 			if (!mad_priv) {
2565 				printk(KERN_ERR PFX "No memory for receive buffer\n");
2566 				ret = -ENOMEM;
2567 				break;
2568 			}
2569 		}
2570 		sg_list.addr = ib_dma_map_single(qp_info->port_priv->device,
2571 						 &mad_priv->grh,
2572 						 sizeof *mad_priv -
2573 						   sizeof mad_priv->header,
2574 						 DMA_FROM_DEVICE);
2575 		mad_priv->header.mapping = sg_list.addr;
2576 		recv_wr.wr_id = (unsigned long)&mad_priv->header.mad_list;
2577 		mad_priv->header.mad_list.mad_queue = recv_queue;
2578 
2579 		/* Post receive WR */
2580 		spin_lock_irqsave(&recv_queue->lock, flags);
2581 		post = (++recv_queue->count < recv_queue->max_active);
2582 		list_add_tail(&mad_priv->header.mad_list.list, &recv_queue->list);
2583 		spin_unlock_irqrestore(&recv_queue->lock, flags);
2584 		ret = ib_post_recv(qp_info->qp, &recv_wr, &bad_recv_wr);
2585 		if (ret) {
2586 			spin_lock_irqsave(&recv_queue->lock, flags);
2587 			list_del(&mad_priv->header.mad_list.list);
2588 			recv_queue->count--;
2589 			spin_unlock_irqrestore(&recv_queue->lock, flags);
2590 			ib_dma_unmap_single(qp_info->port_priv->device,
2591 					    mad_priv->header.mapping,
2592 					    sizeof *mad_priv -
2593 					      sizeof mad_priv->header,
2594 					    DMA_FROM_DEVICE);
2595 			kmem_cache_free(ib_mad_cache, mad_priv);
2596 			printk(KERN_ERR PFX "ib_post_recv failed: %d\n", ret);
2597 			break;
2598 		}
2599 	} while (post);
2600 
2601 	return ret;
2602 }
2603 
2604 /*
2605  * Return all the posted receive MADs
2606  */
2607 static void cleanup_recv_queue(struct ib_mad_qp_info *qp_info)
2608 {
2609 	struct ib_mad_private_header *mad_priv_hdr;
2610 	struct ib_mad_private *recv;
2611 	struct ib_mad_list_head *mad_list;
2612 
2613 	while (!list_empty(&qp_info->recv_queue.list)) {
2614 
2615 		mad_list = list_entry(qp_info->recv_queue.list.next,
2616 				      struct ib_mad_list_head, list);
2617 		mad_priv_hdr = container_of(mad_list,
2618 					    struct ib_mad_private_header,
2619 					    mad_list);
2620 		recv = container_of(mad_priv_hdr, struct ib_mad_private,
2621 				    header);
2622 
2623 		/* Remove from posted receive MAD list */
2624 		list_del(&mad_list->list);
2625 
2626 		ib_dma_unmap_single(qp_info->port_priv->device,
2627 				    recv->header.mapping,
2628 				    sizeof(struct ib_mad_private) -
2629 				      sizeof(struct ib_mad_private_header),
2630 				    DMA_FROM_DEVICE);
2631 		kmem_cache_free(ib_mad_cache, recv);
2632 	}
2633 
2634 	qp_info->recv_queue.count = 0;
2635 }
2636 
2637 /*
2638  * Start the port
2639  */
2640 static int ib_mad_port_start(struct ib_mad_port_private *port_priv)
2641 {
2642 	int ret, i;
2643 	struct ib_qp_attr *attr;
2644 	struct ib_qp *qp;
2645 
2646 	attr = kmalloc(sizeof *attr, GFP_KERNEL);
2647 	if (!attr) {
2648 		printk(KERN_ERR PFX "Couldn't kmalloc ib_qp_attr\n");
2649 		return -ENOMEM;
2650 	}
2651 
2652 	for (i = 0; i < IB_MAD_QPS_CORE; i++) {
2653 		qp = port_priv->qp_info[i].qp;
2654 		/*
2655 		 * PKey index for QP1 is irrelevant but
2656 		 * one is needed for the Reset to Init transition
2657 		 */
2658 		attr->qp_state = IB_QPS_INIT;
2659 		attr->pkey_index = 0;
2660 		attr->qkey = (qp->qp_num == 0) ? 0 : IB_QP1_QKEY;
2661 		ret = ib_modify_qp(qp, attr, IB_QP_STATE |
2662 					     IB_QP_PKEY_INDEX | IB_QP_QKEY);
2663 		if (ret) {
2664 			printk(KERN_ERR PFX "Couldn't change QP%d state to "
2665 			       "INIT: %d\n", i, ret);
2666 			goto out;
2667 		}
2668 
2669 		attr->qp_state = IB_QPS_RTR;
2670 		ret = ib_modify_qp(qp, attr, IB_QP_STATE);
2671 		if (ret) {
2672 			printk(KERN_ERR PFX "Couldn't change QP%d state to "
2673 			       "RTR: %d\n", i, ret);
2674 			goto out;
2675 		}
2676 
2677 		attr->qp_state = IB_QPS_RTS;
2678 		attr->sq_psn = IB_MAD_SEND_Q_PSN;
2679 		ret = ib_modify_qp(qp, attr, IB_QP_STATE | IB_QP_SQ_PSN);
2680 		if (ret) {
2681 			printk(KERN_ERR PFX "Couldn't change QP%d state to "
2682 			       "RTS: %d\n", i, ret);
2683 			goto out;
2684 		}
2685 	}
2686 
2687 	ret = ib_req_notify_cq(port_priv->cq, IB_CQ_NEXT_COMP);
2688 	if (ret) {
2689 		printk(KERN_ERR PFX "Failed to request completion "
2690 		       "notification: %d\n", ret);
2691 		goto out;
2692 	}
2693 
2694 	for (i = 0; i < IB_MAD_QPS_CORE; i++) {
2695 		ret = ib_mad_post_receive_mads(&port_priv->qp_info[i], NULL);
2696 		if (ret) {
2697 			printk(KERN_ERR PFX "Couldn't post receive WRs\n");
2698 			goto out;
2699 		}
2700 	}
2701 out:
2702 	kfree(attr);
2703 	return ret;
2704 }
2705 
2706 static void qp_event_handler(struct ib_event *event, void *qp_context)
2707 {
2708 	struct ib_mad_qp_info	*qp_info = qp_context;
2709 
2710 	/* It's worse than that! He's dead, Jim! */
2711 	printk(KERN_ERR PFX "Fatal error (%d) on MAD QP (%d)\n",
2712 		event->event, qp_info->qp->qp_num);
2713 }
2714 
2715 static void init_mad_queue(struct ib_mad_qp_info *qp_info,
2716 			   struct ib_mad_queue *mad_queue)
2717 {
2718 	mad_queue->qp_info = qp_info;
2719 	mad_queue->count = 0;
2720 	spin_lock_init(&mad_queue->lock);
2721 	INIT_LIST_HEAD(&mad_queue->list);
2722 }
2723 
2724 static void init_mad_qp(struct ib_mad_port_private *port_priv,
2725 			struct ib_mad_qp_info *qp_info)
2726 {
2727 	qp_info->port_priv = port_priv;
2728 	init_mad_queue(qp_info, &qp_info->send_queue);
2729 	init_mad_queue(qp_info, &qp_info->recv_queue);
2730 	INIT_LIST_HEAD(&qp_info->overflow_list);
2731 	spin_lock_init(&qp_info->snoop_lock);
2732 	qp_info->snoop_table = NULL;
2733 	qp_info->snoop_table_size = 0;
2734 	atomic_set(&qp_info->snoop_count, 0);
2735 }
2736 
2737 static int create_mad_qp(struct ib_mad_qp_info *qp_info,
2738 			 enum ib_qp_type qp_type)
2739 {
2740 	struct ib_qp_init_attr	qp_init_attr;
2741 	int ret;
2742 
2743 	memset(&qp_init_attr, 0, sizeof qp_init_attr);
2744 	qp_init_attr.send_cq = qp_info->port_priv->cq;
2745 	qp_init_attr.recv_cq = qp_info->port_priv->cq;
2746 	qp_init_attr.sq_sig_type = IB_SIGNAL_ALL_WR;
2747 	qp_init_attr.cap.max_send_wr = mad_sendq_size;
2748 	qp_init_attr.cap.max_recv_wr = mad_recvq_size;
2749 	qp_init_attr.cap.max_send_sge = IB_MAD_SEND_REQ_MAX_SG;
2750 	qp_init_attr.cap.max_recv_sge = IB_MAD_RECV_REQ_MAX_SG;
2751 	qp_init_attr.qp_type = qp_type;
2752 	qp_init_attr.port_num = qp_info->port_priv->port_num;
2753 	qp_init_attr.qp_context = qp_info;
2754 	qp_init_attr.event_handler = qp_event_handler;
2755 	qp_info->qp = ib_create_qp(qp_info->port_priv->pd, &qp_init_attr);
2756 	if (IS_ERR(qp_info->qp)) {
2757 		printk(KERN_ERR PFX "Couldn't create ib_mad QP%d\n",
2758 		       get_spl_qp_index(qp_type));
2759 		ret = PTR_ERR(qp_info->qp);
2760 		goto error;
2761 	}
2762 	/* Use minimum queue sizes unless the CQ is resized */
2763 	qp_info->send_queue.max_active = mad_sendq_size;
2764 	qp_info->recv_queue.max_active = mad_recvq_size;
2765 	return 0;
2766 
2767 error:
2768 	return ret;
2769 }
2770 
2771 static void destroy_mad_qp(struct ib_mad_qp_info *qp_info)
2772 {
2773 	ib_destroy_qp(qp_info->qp);
2774 	kfree(qp_info->snoop_table);
2775 }
2776 
2777 /*
2778  * Open the port
2779  * Create the QP, PD, MR, and CQ if needed
2780  */
2781 static int ib_mad_port_open(struct ib_device *device,
2782 			    int port_num)
2783 {
2784 	int ret, cq_size;
2785 	struct ib_mad_port_private *port_priv;
2786 	unsigned long flags;
2787 	char name[sizeof "ib_mad123"];
2788 
2789 	/* Create new device info */
2790 	port_priv = kzalloc(sizeof *port_priv, GFP_KERNEL);
2791 	if (!port_priv) {
2792 		printk(KERN_ERR PFX "No memory for ib_mad_port_private\n");
2793 		return -ENOMEM;
2794 	}
2795 
2796 	port_priv->device = device;
2797 	port_priv->port_num = port_num;
2798 	spin_lock_init(&port_priv->reg_lock);
2799 	INIT_LIST_HEAD(&port_priv->agent_list);
2800 	init_mad_qp(port_priv, &port_priv->qp_info[0]);
2801 	init_mad_qp(port_priv, &port_priv->qp_info[1]);
2802 
2803 	cq_size = (mad_sendq_size + mad_recvq_size) * 2;
2804 	port_priv->cq = ib_create_cq(port_priv->device,
2805 				     ib_mad_thread_completion_handler,
2806 				     NULL, port_priv, cq_size, 0);
2807 	if (IS_ERR(port_priv->cq)) {
2808 		printk(KERN_ERR PFX "Couldn't create ib_mad CQ\n");
2809 		ret = PTR_ERR(port_priv->cq);
2810 		goto error3;
2811 	}
2812 
2813 	port_priv->pd = ib_alloc_pd(device);
2814 	if (IS_ERR(port_priv->pd)) {
2815 		printk(KERN_ERR PFX "Couldn't create ib_mad PD\n");
2816 		ret = PTR_ERR(port_priv->pd);
2817 		goto error4;
2818 	}
2819 
2820 	port_priv->mr = ib_get_dma_mr(port_priv->pd, IB_ACCESS_LOCAL_WRITE);
2821 	if (IS_ERR(port_priv->mr)) {
2822 		printk(KERN_ERR PFX "Couldn't get ib_mad DMA MR\n");
2823 		ret = PTR_ERR(port_priv->mr);
2824 		goto error5;
2825 	}
2826 
2827 	ret = create_mad_qp(&port_priv->qp_info[0], IB_QPT_SMI);
2828 	if (ret)
2829 		goto error6;
2830 	ret = create_mad_qp(&port_priv->qp_info[1], IB_QPT_GSI);
2831 	if (ret)
2832 		goto error7;
2833 
2834 	snprintf(name, sizeof name, "ib_mad%d", port_num);
2835 	port_priv->wq = create_singlethread_workqueue(name);
2836 	if (!port_priv->wq) {
2837 		ret = -ENOMEM;
2838 		goto error8;
2839 	}
2840 	INIT_WORK(&port_priv->work, ib_mad_completion_handler);
2841 
2842 	spin_lock_irqsave(&ib_mad_port_list_lock, flags);
2843 	list_add_tail(&port_priv->port_list, &ib_mad_port_list);
2844 	spin_unlock_irqrestore(&ib_mad_port_list_lock, flags);
2845 
2846 	ret = ib_mad_port_start(port_priv);
2847 	if (ret) {
2848 		printk(KERN_ERR PFX "Couldn't start port\n");
2849 		goto error9;
2850 	}
2851 
2852 	return 0;
2853 
2854 error9:
2855 	spin_lock_irqsave(&ib_mad_port_list_lock, flags);
2856 	list_del_init(&port_priv->port_list);
2857 	spin_unlock_irqrestore(&ib_mad_port_list_lock, flags);
2858 
2859 	destroy_workqueue(port_priv->wq);
2860 error8:
2861 	destroy_mad_qp(&port_priv->qp_info[1]);
2862 error7:
2863 	destroy_mad_qp(&port_priv->qp_info[0]);
2864 error6:
2865 	ib_dereg_mr(port_priv->mr);
2866 error5:
2867 	ib_dealloc_pd(port_priv->pd);
2868 error4:
2869 	ib_destroy_cq(port_priv->cq);
2870 	cleanup_recv_queue(&port_priv->qp_info[1]);
2871 	cleanup_recv_queue(&port_priv->qp_info[0]);
2872 error3:
2873 	kfree(port_priv);
2874 
2875 	return ret;
2876 }
2877 
2878 /*
2879  * Close the port
2880  * If there are no classes using the port, free the port
2881  * resources (CQ, MR, PD, QP) and remove the port's info structure
2882  */
2883 static int ib_mad_port_close(struct ib_device *device, int port_num)
2884 {
2885 	struct ib_mad_port_private *port_priv;
2886 	unsigned long flags;
2887 
2888 	spin_lock_irqsave(&ib_mad_port_list_lock, flags);
2889 	port_priv = __ib_get_mad_port(device, port_num);
2890 	if (port_priv == NULL) {
2891 		spin_unlock_irqrestore(&ib_mad_port_list_lock, flags);
2892 		printk(KERN_ERR PFX "Port %d not found\n", port_num);
2893 		return -ENODEV;
2894 	}
2895 	list_del_init(&port_priv->port_list);
2896 	spin_unlock_irqrestore(&ib_mad_port_list_lock, flags);
2897 
2898 	destroy_workqueue(port_priv->wq);
2899 	destroy_mad_qp(&port_priv->qp_info[1]);
2900 	destroy_mad_qp(&port_priv->qp_info[0]);
2901 	ib_dereg_mr(port_priv->mr);
2902 	ib_dealloc_pd(port_priv->pd);
2903 	ib_destroy_cq(port_priv->cq);
2904 	cleanup_recv_queue(&port_priv->qp_info[1]);
2905 	cleanup_recv_queue(&port_priv->qp_info[0]);
2906 	/* XXX: Handle deallocation of MAD registration tables */
2907 
2908 	kfree(port_priv);
2909 
2910 	return 0;
2911 }
2912 
2913 static void ib_mad_init_device(struct ib_device *device)
2914 {
2915 	int start, end, i;
2916 
2917 	if (rdma_node_get_transport(device->node_type) != RDMA_TRANSPORT_IB)
2918 		return;
2919 
2920 	if (device->node_type == RDMA_NODE_IB_SWITCH) {
2921 		start = 0;
2922 		end   = 0;
2923 	} else {
2924 		start = 1;
2925 		end   = device->phys_port_cnt;
2926 	}
2927 
2928 	for (i = start; i <= end; i++) {
2929 		if (ib_mad_port_open(device, i)) {
2930 			printk(KERN_ERR PFX "Couldn't open %s port %d\n",
2931 			       device->name, i);
2932 			goto error;
2933 		}
2934 		if (ib_agent_port_open(device, i)) {
2935 			printk(KERN_ERR PFX "Couldn't open %s port %d "
2936 			       "for agents\n",
2937 			       device->name, i);
2938 			goto error_agent;
2939 		}
2940 	}
2941 	return;
2942 
2943 error_agent:
2944 	if (ib_mad_port_close(device, i))
2945 		printk(KERN_ERR PFX "Couldn't close %s port %d\n",
2946 		       device->name, i);
2947 
2948 error:
2949 	i--;
2950 
2951 	while (i >= start) {
2952 		if (ib_agent_port_close(device, i))
2953 			printk(KERN_ERR PFX "Couldn't close %s port %d "
2954 			       "for agents\n",
2955 			       device->name, i);
2956 		if (ib_mad_port_close(device, i))
2957 			printk(KERN_ERR PFX "Couldn't close %s port %d\n",
2958 			       device->name, i);
2959 		i--;
2960 	}
2961 }
2962 
2963 static void ib_mad_remove_device(struct ib_device *device)
2964 {
2965 	int i, num_ports, cur_port;
2966 
2967 	if (device->node_type == RDMA_NODE_IB_SWITCH) {
2968 		num_ports = 1;
2969 		cur_port = 0;
2970 	} else {
2971 		num_ports = device->phys_port_cnt;
2972 		cur_port = 1;
2973 	}
2974 	for (i = 0; i < num_ports; i++, cur_port++) {
2975 		if (ib_agent_port_close(device, cur_port))
2976 			printk(KERN_ERR PFX "Couldn't close %s port %d "
2977 			       "for agents\n",
2978 			       device->name, cur_port);
2979 		if (ib_mad_port_close(device, cur_port))
2980 			printk(KERN_ERR PFX "Couldn't close %s port %d\n",
2981 			       device->name, cur_port);
2982 	}
2983 }
2984 
2985 static struct ib_client mad_client = {
2986 	.name   = "mad",
2987 	.add = ib_mad_init_device,
2988 	.remove = ib_mad_remove_device
2989 };
2990 
2991 static int __init ib_mad_init_module(void)
2992 {
2993 	int ret;
2994 
2995 	mad_recvq_size = min(mad_recvq_size, IB_MAD_QP_MAX_SIZE);
2996 	mad_recvq_size = max(mad_recvq_size, IB_MAD_QP_MIN_SIZE);
2997 
2998 	mad_sendq_size = min(mad_sendq_size, IB_MAD_QP_MAX_SIZE);
2999 	mad_sendq_size = max(mad_sendq_size, IB_MAD_QP_MIN_SIZE);
3000 
3001 	ib_mad_cache = kmem_cache_create("ib_mad",
3002 					 sizeof(struct ib_mad_private),
3003 					 0,
3004 					 SLAB_HWCACHE_ALIGN,
3005 					 NULL);
3006 	if (!ib_mad_cache) {
3007 		printk(KERN_ERR PFX "Couldn't create ib_mad cache\n");
3008 		ret = -ENOMEM;
3009 		goto error1;
3010 	}
3011 
3012 	INIT_LIST_HEAD(&ib_mad_port_list);
3013 
3014 	if (ib_register_client(&mad_client)) {
3015 		printk(KERN_ERR PFX "Couldn't register ib_mad client\n");
3016 		ret = -EINVAL;
3017 		goto error2;
3018 	}
3019 
3020 	return 0;
3021 
3022 error2:
3023 	kmem_cache_destroy(ib_mad_cache);
3024 error1:
3025 	return ret;
3026 }
3027 
3028 static void __exit ib_mad_cleanup_module(void)
3029 {
3030 	ib_unregister_client(&mad_client);
3031 	kmem_cache_destroy(ib_mad_cache);
3032 }
3033 
3034 module_init(ib_mad_init_module);
3035 module_exit(ib_mad_cleanup_module);
3036