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