drivers/infiniband/core/mad.c

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