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