drivers/infiniband/core/mad.c

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