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
  37 #include "mad_priv.h"
  38 #include "smi.h"
  39 #include "agent.h"
  40
  41 MODULE_LICENSE("Dual BSD/GPL");
  42 MODULE_DESCRIPTION("kernel IB MAD API");
  43 MODULE_AUTHOR("Hal Rosenstock");
  44 MODULE_AUTHOR("Sean Hefty");
  45
  46
  47 kmem_cache_t *ib_mad_cache;
  48 static struct list_head ib_mad_port_list;
  49 static u32 ib_mad_client_id = 0;
  50
  51 /* Port list lock */
  52 static spinlock_t ib_mad_port_list_lock;
  53
  54
  55 /* Forward declarations */
  56 static int method_in_use(struct ib_mad_mgmt_method_table **method,
  57                          struct ib_mad_reg_req *mad_reg_req);
  58 static void remove_mad_reg_req(struct ib_mad_agent_private *priv);
  59 static struct ib_mad_agent_private *find_mad_agent(
  60                                         struct ib_mad_port_private *port_priv,
  61                                         struct ib_mad *mad, int solicited);
  62 static int ib_mad_post_receive_mads(struct ib_mad_qp_info *qp_info,
  63                                     struct ib_mad_private *mad);
  64 static void cancel_mads(struct ib_mad_agent_private *mad_agent_priv);
  65 static void ib_mad_complete_send_wr(struct ib_mad_send_wr_private *mad_send_wr,
  66                                     struct ib_mad_send_wc *mad_send_wc);
  67 static void timeout_sends(void *data);
  68 static void cancel_sends(void *data);
  69 static void local_completions(void *data);
  70 static int solicited_mad(struct ib_mad *mad);
  71 static int add_nonoui_reg_req(struct ib_mad_reg_req *mad_reg_req,
  72                               struct ib_mad_agent_private *agent_priv,
  73                               u8 mgmt_class);
  74 static int add_oui_reg_req(struct ib_mad_reg_req *mad_reg_req,
  75                            struct ib_mad_agent_private *agent_priv);
  76
  77 /*
  78  * Returns a ib_mad_port_private structure or NULL for a device/port
  79  * Assumes ib_mad_port_list_lock is being held
  80  */
  81 static inline struct ib_mad_port_private *
  82 __ib_get_mad_port(struct ib_device *device, int port_num)
  83 {
  84         struct ib_mad_port_private *entry;
  85
  86         list_for_each_entry(entry, &ib_mad_port_list, port_list) {
  87                 if (entry->device == device && entry->port_num == port_num)
  88                         return entry;
  89         }
  90         return NULL;
  91 }
  92
  93 /*
  94  * Wrapper function to return a ib_mad_port_private structure or NULL
  95  * for a device/port
  96  */
  97 static inline struct ib_mad_port_private *
  98 ib_get_mad_port(struct ib_device *device, int port_num)
  99 {
 100         struct ib_mad_port_private *entry;
 101         unsigned long flags;
 102
 103         spin_lock_irqsave(&ib_mad_port_list_lock, flags);
 104         entry = __ib_get_mad_port(device, port_num);
 105         spin_unlock_irqrestore(&ib_mad_port_list_lock, flags);
 106
 107         return entry;
 108 }
 109
 110 static inline u8 convert_mgmt_class(u8 mgmt_class)
 111 {
 112         /* Alias IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE to 0 */
 113         return mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE ?
 114                 0 : mgmt_class;
 115 }
 116
 117 static int get_spl_qp_index(enum ib_qp_type qp_type)
 118 {
 119         switch (qp_type)
 120         {
 121         case IB_QPT_SMI:
 122                 return 0;
 123         case IB_QPT_GSI:
 124                 return 1;
 125         default:
 126                 return -1;
 127         }
 128 }
 129
 130 static int vendor_class_index(u8 mgmt_class)
 131 {
 132         return mgmt_class - IB_MGMT_CLASS_VENDOR_RANGE2_START;
 133 }
 134
 135 static int is_vendor_class(u8 mgmt_class)
 136 {
 137         if ((mgmt_class < IB_MGMT_CLASS_VENDOR_RANGE2_START) ||
 138             (mgmt_class > IB_MGMT_CLASS_VENDOR_RANGE2_END))
 139                 return 0;
 140         return 1;
 141 }
 142
 143 static int is_vendor_oui(char *oui)
 144 {
 145         if (oui[0] || oui[1] || oui[2])
 146                 return 1;
 147         return 0;
 148 }
 149
 150 static int is_vendor_method_in_use(
 151                 struct ib_mad_mgmt_vendor_class *vendor_class,
 152                 struct ib_mad_reg_req *mad_reg_req)
 153 {
 154         struct ib_mad_mgmt_method_table *method;
 155         int i;
 156
 157         for (i = 0; i < MAX_MGMT_OUI; i++) {
 158                 if (!memcmp(vendor_class->oui[i], mad_reg_req->oui, 3)) {
 159                         method = vendor_class->method_table[i];
 160                         if (method) {
 161                                 if (method_in_use(&method, mad_reg_req))
 162                                         return 1;
 163                                 else
 164                                         break;
 165                         }
 166                 }
 167         }
 168         return 0;
 169 }
 170
 171 /*
 172  * ib_register_mad_agent - Register to send/receive MADs
 173  */
 174 struct ib_mad_agent *ib_register_mad_agent(struct ib_device *device,
 175                                            u8 port_num,
 176                                            enum ib_qp_type qp_type,
 177                                            struct ib_mad_reg_req *mad_reg_req,
 178                                            u8 rmpp_version,
 179                                            ib_mad_send_handler send_handler,
 180                                            ib_mad_recv_handler recv_handler,
 181                                            void *context)
 182 {
 183         struct ib_mad_port_private *port_priv;
 184         struct ib_mad_agent *ret = ERR_PTR(-EINVAL);
 185         struct ib_mad_agent_private *mad_agent_priv;
 186         struct ib_mad_reg_req *reg_req = NULL;
 187         struct ib_mad_mgmt_class_table *class;
 188         struct ib_mad_mgmt_vendor_class_table *vendor;
 189         struct ib_mad_mgmt_vendor_class *vendor_class;
 190         struct ib_mad_mgmt_method_table *method;
 191         int ret2, qpn;
 192         unsigned long flags;
 193         u8 mgmt_class, vclass;
 194
 195         /* Validate parameters */
 196         qpn = get_spl_qp_index(qp_type);
 197         if (qpn == -1)
 198                 goto error1;
 199
 200         if (rmpp_version)
 201                 goto error1;    /* XXX: until RMPP implemented */
 202
 203         /* Validate MAD registration request if supplied */
 204         if (mad_reg_req) {
 205                 if (mad_reg_req->mgmt_class_version >= MAX_MGMT_VERSION)
 206                         goto error1;
 207                 if (!recv_handler)
 208                         goto error1;
 209                 if (mad_reg_req->mgmt_class >= MAX_MGMT_CLASS) {
 210                         /*
 211                          * IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE is the only
 212                          * one in this range currently allowed
 213                          */
 214                         if (mad_reg_req->mgmt_class !=
 215                             IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE)
 216                                 goto error1;
 217                 } else if (mad_reg_req->mgmt_class == 0) {
 218                         /*
 219                          * Class 0 is reserved in IBA and is used for
 220                          * aliasing of IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE
 221                          */
 222                         goto error1;
 223                 } else if (is_vendor_class(mad_reg_req->mgmt_class)) {
 224                         /*
 225                          * If class is in "new" vendor range,
 226                          * ensure supplied OUI is not zero
 227                          */
 228                         if (!is_vendor_oui(mad_reg_req->oui))
 229                                 goto error1;
 230                 }
 231                 /* Make sure class supplied is consistent with QP type */
 232                 if (qp_type == IB_QPT_SMI) {
 233                         if ((mad_reg_req->mgmt_class !=
 234                                         IB_MGMT_CLASS_SUBN_LID_ROUTED) &&
 235                             (mad_reg_req->mgmt_class !=
 236                                         IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE))
 237                                 goto error1;
 238                 } else {
 239                         if ((mad_reg_req->mgmt_class ==
 240                                         IB_MGMT_CLASS_SUBN_LID_ROUTED) ||
 241                             (mad_reg_req->mgmt_class ==
 242                                         IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE))
 243                                 goto error1;
 244                 }
 245         } else {
 246                 /* No registration request supplied */
 247                 if (!send_handler)
 248                         goto error1;
 249         }
 250
 251         /* Validate device and port */
 252         port_priv = ib_get_mad_port(device, port_num);
 253         if (!port_priv) {
 254                 ret = ERR_PTR(-ENODEV);
 255                 goto error1;
 256         }
 257
 258         /* Allocate structures */
 259         mad_agent_priv = kmalloc(sizeof *mad_agent_priv, GFP_KERNEL);
 260         if (!mad_agent_priv) {
 261                 ret = ERR_PTR(-ENOMEM);
 262                 goto error1;
 263         }
 264
 265         if (mad_reg_req) {
 266                 reg_req = kmalloc(sizeof *reg_req, GFP_KERNEL);
 267                 if (!reg_req) {
 268                         ret = ERR_PTR(-ENOMEM);
 269                         goto error2;
 270                 }
 271                 /* Make a copy of the MAD registration request */
 272                 memcpy(reg_req, mad_reg_req, sizeof *reg_req);
 273         }
 274
 275         /* Now, fill in the various structures */
 276         memset(mad_agent_priv, 0, sizeof *mad_agent_priv);
 277         mad_agent_priv->qp_info = &port_priv->qp_info[qpn];
 278         mad_agent_priv->reg_req = reg_req;
 279         mad_agent_priv->rmpp_version = rmpp_version;
 280         mad_agent_priv->agent.device = device;
 281         mad_agent_priv->agent.recv_handler = recv_handler;
 282         mad_agent_priv->agent.send_handler = send_handler;
 283         mad_agent_priv->agent.context = context;
 284         mad_agent_priv->agent.qp = port_priv->qp_info[qpn].qp;
 285         mad_agent_priv->agent.port_num = port_num;
 286
 287         spin_lock_irqsave(&port_priv->reg_lock, flags);
 288         mad_agent_priv->agent.hi_tid = ++ib_mad_client_id;
 289
 290         /*
 291          * Make sure MAD registration (if supplied)
 292          * is non overlapping with any existing ones
 293          */
 294         if (mad_reg_req) {
 295                 mgmt_class = convert_mgmt_class(mad_reg_req->mgmt_class);
 296                 if (!is_vendor_class(mgmt_class)) {
 297                         class = port_priv->version[mad_reg_req->
 298                                                    mgmt_class_version].class;
 299                         if (class) {
 300                                 method = class->method_table[mgmt_class];
 301                                 if (method) {
 302                                         if (method_in_use(&method,
 303                                                            mad_reg_req))
 304                                                 goto error3;
 305                                 }
 306                         }
 307                         ret2 = add_nonoui_reg_req(mad_reg_req, mad_agent_priv,
 308                                                   mgmt_class);
 309                 } else {
 310                         /* "New" vendor class range */
 311                         vendor = port_priv->version[mad_reg_req->
 312                                                     mgmt_class_version].vendor;
 313                         if (vendor) {
 314                                 vclass = vendor_class_index(mgmt_class);
 315                                 vendor_class = vendor->vendor_class[vclass];
 316                                 if (vendor_class) {
 317                                         if (is_vendor_method_in_use(
 318                                                         vendor_class,
 319                                                         mad_reg_req))
 320                                                 goto error3;
 321                                 }
 322                         }
 323                         ret2 = add_oui_reg_req(mad_reg_req, mad_agent_priv);
 324                 }
 325                 if (ret2) {
 326                         ret = ERR_PTR(ret2);
 327                         goto error3;
 328                 }
 329         }
 330
 331         /* Add mad agent into port's agent list */
 332         list_add_tail(&mad_agent_priv->agent_list, &port_priv->agent_list);
 333         spin_unlock_irqrestore(&port_priv->reg_lock, flags);
 334
 335         spin_lock_init(&mad_agent_priv->lock);
 336         INIT_LIST_HEAD(&mad_agent_priv->send_list);
 337         INIT_LIST_HEAD(&mad_agent_priv->wait_list);
 338         INIT_WORK(&mad_agent_priv->timed_work, timeout_sends, mad_agent_priv);
 339         INIT_LIST_HEAD(&mad_agent_priv->local_list);
 340         INIT_WORK(&mad_agent_priv->local_work, local_completions,
 341                    mad_agent_priv);
 342         INIT_LIST_HEAD(&mad_agent_priv->canceled_list);
 343         INIT_WORK(&mad_agent_priv->canceled_work, cancel_sends, mad_agent_priv);
 344         atomic_set(&mad_agent_priv->refcount, 1);
 345         init_waitqueue_head(&mad_agent_priv->wait);
 346
 347         return &mad_agent_priv->agent;
 348
 349 error3:
 350         spin_unlock_irqrestore(&port_priv->reg_lock, flags);
 351         kfree(reg_req);
 352 error2:
 353         kfree(mad_agent_priv);
 354 error1:
 355         return ret;
 356 }
 357 EXPORT_SYMBOL(ib_register_mad_agent);
 358
 359 static inline int is_snooping_sends(int mad_snoop_flags)
 360 {
 361         return (mad_snoop_flags &
 362                 (/*IB_MAD_SNOOP_POSTED_SENDS |
 363                  IB_MAD_SNOOP_RMPP_SENDS |*/
 364                  IB_MAD_SNOOP_SEND_COMPLETIONS /*|
 365                  IB_MAD_SNOOP_RMPP_SEND_COMPLETIONS*/));
 366 }
 367
 368 static inline int is_snooping_recvs(int mad_snoop_flags)
 369 {
 370         return (mad_snoop_flags &
 371                 (IB_MAD_SNOOP_RECVS /*|
 372                  IB_MAD_SNOOP_RMPP_RECVS*/));
 373 }
 374
 375 static int register_snoop_agent(struct ib_mad_qp_info *qp_info,
 376                                 struct ib_mad_snoop_private *mad_snoop_priv)
 377 {
 378         struct ib_mad_snoop_private **new_snoop_table;
 379         unsigned long flags;
 380         int i;
 381
 382         spin_lock_irqsave(&qp_info->snoop_lock, flags);
 383         /* Check for empty slot in array. */
 384         for (i = 0; i < qp_info->snoop_table_size; i++)
 385                 if (!qp_info->snoop_table[i])
 386                         break;
 387
 388         if (i == qp_info->snoop_table_size) {
 389                 /* Grow table. */
 390                 new_snoop_table = kmalloc(sizeof mad_snoop_priv *
 391                                           qp_info->snoop_table_size + 1,
 392                                           GFP_ATOMIC);
 393                 if (!new_snoop_table) {
 394                         i = -ENOMEM;
 395                         goto out;
 396                 }
 397                 if (qp_info->snoop_table) {
 398                         memcpy(new_snoop_table, qp_info->snoop_table,
 399                                sizeof mad_snoop_priv *
 400                                qp_info->snoop_table_size);
 401                         kfree(qp_info->snoop_table);
 402                 }
 403                 qp_info->snoop_table = new_snoop_table;
 404                 qp_info->snoop_table_size++;
 405         }
 406         qp_info->snoop_table[i] = mad_snoop_priv;
 407         atomic_inc(&qp_info->snoop_count);
 408 out:
 409         spin_unlock_irqrestore(&qp_info->snoop_lock, flags);
 410         return i;
 411 }
 412
 413 struct ib_mad_agent *ib_register_mad_snoop(struct ib_device *device,
 414                                            u8 port_num,
 415                                            enum ib_qp_type qp_type,
 416                                            int mad_snoop_flags,
 417                                            ib_mad_snoop_handler snoop_handler,
 418                                            ib_mad_recv_handler recv_handler,
 419                                            void *context)
 420 {
 421         struct ib_mad_port_private *port_priv;
 422         struct ib_mad_agent *ret;
 423         struct ib_mad_snoop_private *mad_snoop_priv;
 424         int qpn;
 425
 426         /* Validate parameters */
 427         if ((is_snooping_sends(mad_snoop_flags) && !snoop_handler) ||
 428             (is_snooping_recvs(mad_snoop_flags) && !recv_handler)) {
 429                 ret = ERR_PTR(-EINVAL);
 430                 goto error1;
 431         }
 432         qpn = get_spl_qp_index(qp_type);
 433         if (qpn == -1) {
 434                 ret = ERR_PTR(-EINVAL);
 435                 goto error1;
 436         }
 437         port_priv = ib_get_mad_port(device, port_num);
 438         if (!port_priv) {
 439                 ret = ERR_PTR(-ENODEV);
 440                 goto error1;
 441         }
 442         /* Allocate structures */
 443         mad_snoop_priv = kmalloc(sizeof *mad_snoop_priv, GFP_KERNEL);
 444         if (!mad_snoop_priv) {
 445                 ret = ERR_PTR(-ENOMEM);
 446                 goto error1;
 447         }
 448
 449         /* Now, fill in the various structures */
 450         memset(mad_snoop_priv, 0, sizeof *mad_snoop_priv);
 451         mad_snoop_priv->qp_info = &port_priv->qp_info[qpn];
 452         mad_snoop_priv->agent.device = device;
 453         mad_snoop_priv->agent.recv_handler = recv_handler;
 454         mad_snoop_priv->agent.snoop_handler = snoop_handler;
 455         mad_snoop_priv->agent.context = context;
 456         mad_snoop_priv->agent.qp = port_priv->qp_info[qpn].qp;
 457         mad_snoop_priv->agent.port_num = port_num;
 458         mad_snoop_priv->mad_snoop_flags = mad_snoop_flags;
 459         init_waitqueue_head(&mad_snoop_priv->wait);
 460         mad_snoop_priv->snoop_index = register_snoop_agent(
 461                                                 &port_priv->qp_info[qpn],
 462                                                 mad_snoop_priv);
 463         if (mad_snoop_priv->snoop_index < 0) {
 464                 ret = ERR_PTR(mad_snoop_priv->snoop_index);
 465                 goto error2;
 466         }
 467
 468         atomic_set(&mad_snoop_priv->refcount, 1);
 469         return &mad_snoop_priv->agent;
 470
 471 error2:
 472         kfree(mad_snoop_priv);
 473 error1:
 474         return ret;
 475 }
 476 EXPORT_SYMBOL(ib_register_mad_snoop);
 477
 478 static void unregister_mad_agent(struct ib_mad_agent_private *mad_agent_priv)
 479 {
 480         struct ib_mad_port_private *port_priv;
 481         unsigned long flags;
 482
 483         /* Note that we could still be handling received MADs */
 484
 485         /*
 486          * Canceling all sends results in dropping received response
 487          * MADs, preventing us from queuing additional work
 488          */
 489         cancel_mads(mad_agent_priv);
 490
 491         port_priv = mad_agent_priv->qp_info->port_priv;
 492
 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, 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         local = kmalloc(sizeof *local, GFP_ATOMIC);
 667         if (!local) {
 668                 ret = -ENOMEM;
 669                 printk(KERN_ERR PFX "No memory for ib_mad_local_private\n");
 670                 goto out;
 671         }
 672         local->mad_priv = NULL;
 673         local->recv_mad_agent = NULL;
 674         mad_priv = kmem_cache_alloc(ib_mad_cache, GFP_ATOMIC);
 675         if (!mad_priv) {
 676                 ret = -ENOMEM;
 677                 printk(KERN_ERR PFX "No memory for local response MAD\n");
 678                 kfree(local);
 679                 goto out;
 680         }
 681
 682         build_smp_wc(send_wr->wr_id, smp->dr_slid, send_wr->wr.ud.pkey_index,
 683                      send_wr->wr.ud.port_num, &mad_wc);
 684
 685         /* No GRH for DR SMP */
 686         ret = device->process_mad(device, 0, port_num, &mad_wc, NULL,
 687                                   (struct ib_mad *)smp,
 688                                   (struct ib_mad *)&mad_priv->mad);
 689         switch (ret)
 690         {
 691         case IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_REPLY:
 692                 /*
 693                  * See if response is solicited and
 694                  * there is a recv handler
 695                  */
 696                 if (solicited_mad(&mad_priv->mad.mad) &&
 697                     mad_agent_priv->agent.recv_handler) {
 698                         local->mad_priv = mad_priv;
 699                         local->recv_mad_agent = mad_agent_priv;
 700                         /*
 701                          * Reference MAD agent until receive
 702                          * side of local completion handled
 703                          */
 704                         atomic_inc(&mad_agent_priv->refcount);
 705                 } else
 706                         kmem_cache_free(ib_mad_cache, mad_priv);
 707                 break;
 708         case IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_CONSUMED:
 709                 kmem_cache_free(ib_mad_cache, mad_priv);
 710                 break;
 711         case IB_MAD_RESULT_SUCCESS:
 712                 /* Treat like an incoming receive MAD */
 713                 solicited = solicited_mad(&mad_priv->mad.mad);
 714                 port_priv = ib_get_mad_port(mad_agent_priv->agent.device,
 715                                             mad_agent_priv->agent.port_num);
 716                 if (port_priv) {
 717                         mad_priv->mad.mad.mad_hdr.tid =
 718                                 ((struct ib_mad *)smp)->mad_hdr.tid;
 719                         recv_mad_agent = find_mad_agent(port_priv,
 720                                                        &mad_priv->mad.mad,
 721                                                         solicited);
 722                 }
 723                 if (!port_priv || !recv_mad_agent) {
 724                         kmem_cache_free(ib_mad_cache, mad_priv);
 725                         kfree(local);
 726                         ret = 0;
 727                         goto out;
 728                 }
 729                 local->mad_priv = mad_priv;
 730                 local->recv_mad_agent = recv_mad_agent;
 731                 break;
 732         default:
 733                 kmem_cache_free(ib_mad_cache, mad_priv);
 734                 kfree(local);
 735                 ret = -EINVAL;
 736                 goto out;
 737         }
 738
 739         local->send_wr = *send_wr;
 740         local->send_wr.sg_list = local->sg_list;
 741         memcpy(local->sg_list, send_wr->sg_list,
 742                sizeof *send_wr->sg_list * send_wr->num_sge);
 743         local->send_wr.next = NULL;
 744         local->tid = send_wr->wr.ud.mad_hdr->tid;
 745         local->wr_id = send_wr->wr_id;
 746         /* Reference MAD agent until send side of local completion handled */
 747         atomic_inc(&mad_agent_priv->refcount);
 748         /* Queue local completion to local list */
 749         spin_lock_irqsave(&mad_agent_priv->lock, flags);
 750         list_add_tail(&local->completion_list, &mad_agent_priv->local_list);
 751         spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
 752         queue_work(mad_agent_priv->qp_info->port_priv->wq,
 753                   &mad_agent_priv->local_work);
 754         ret = 1;
 755 out:
 756         return ret;
 757 }
 758
 759 static int ib_send_mad(struct ib_mad_agent_private *mad_agent_priv,
 760                        struct ib_mad_send_wr_private *mad_send_wr)
 761 {
 762         struct ib_mad_qp_info *qp_info;
 763         struct ib_send_wr *bad_send_wr;
 764         unsigned long flags;
 765         int ret;
 766
 767         /* Replace user's WR ID with our own to find WR upon completion */
 768         qp_info = mad_agent_priv->qp_info;
 769         mad_send_wr->wr_id = mad_send_wr->send_wr.wr_id;
 770         mad_send_wr->send_wr.wr_id = (unsigned long)&mad_send_wr->mad_list;
 771         mad_send_wr->mad_list.mad_queue = &qp_info->send_queue;
 772
 773         spin_lock_irqsave(&qp_info->send_queue.lock, flags);
 774         if (qp_info->send_queue.count++ < qp_info->send_queue.max_active) {
 775                 list_add_tail(&mad_send_wr->mad_list.list,
 776                               &qp_info->send_queue.list);
 777                 spin_unlock_irqrestore(&qp_info->send_queue.lock, flags);
 778                 ret = ib_post_send(mad_agent_priv->agent.qp,
 779                                    &mad_send_wr->send_wr, &bad_send_wr);
 780                 if (ret) {
 781                         printk(KERN_ERR PFX "ib_post_send failed: %d\n", ret);
 782                         dequeue_mad(&mad_send_wr->mad_list);
 783                 }
 784         } else {
 785                 list_add_tail(&mad_send_wr->mad_list.list,
 786                               &qp_info->overflow_list);
 787                 spin_unlock_irqrestore(&qp_info->send_queue.lock, flags);
 788                 ret = 0;
 789         }
 790         return ret;
 791 }
 792
 793 /*
 794  * ib_post_send_mad - Posts MAD(s) to the send queue of the QP associated
 795  *  with the registered client
 796  */
 797 int ib_post_send_mad(struct ib_mad_agent *mad_agent,
 798                      struct ib_send_wr *send_wr,
 799                      struct ib_send_wr **bad_send_wr)
 800 {
 801         int ret = -EINVAL;
 802         struct ib_mad_agent_private *mad_agent_priv;
 803
 804         /* Validate supplied parameters */
 805         if (!bad_send_wr)
 806                 goto error1;
 807
 808         if (!mad_agent || !send_wr)
 809                 goto error2;
 810
 811         if (!mad_agent->send_handler)
 812                 goto error2;
 813
 814         mad_agent_priv = container_of(mad_agent,
 815                                       struct ib_mad_agent_private,
 816                                       agent);
 817
 818         /* Walk list of send WRs and post each on send list */
 819         while (send_wr) {
 820                 unsigned long                   flags;
 821                 struct ib_send_wr               *next_send_wr;
 822                 struct ib_mad_send_wr_private   *mad_send_wr;
 823                 struct ib_smp                   *smp;
 824
 825                 /* Validate more parameters */
 826                 if (send_wr->num_sge > IB_MAD_SEND_REQ_MAX_SG)
 827                         goto error2;
 828
 829                 if (send_wr->wr.ud.timeout_ms && !mad_agent->recv_handler)
 830                         goto error2;
 831
 832                 if (!send_wr->wr.ud.mad_hdr) {
 833                         printk(KERN_ERR PFX "MAD header must be supplied "
 834                                "in WR %p\n", send_wr);
 835                         goto error2;
 836                 }
 837
 838                 /*
 839                  * Save pointer to next work request to post in case the
 840                  * current one completes, and the user modifies the work
 841                  * request associated with the completion
 842                  */
 843                 next_send_wr = (struct ib_send_wr *)send_wr->next;
 844
 845                 smp = (struct ib_smp *)send_wr->wr.ud.mad_hdr;
 846                 if (smp->mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE) {
 847                         ret = handle_outgoing_dr_smp(mad_agent_priv, smp,
 848                                                      send_wr);
 849                         if (ret < 0)            /* error */
 850                                 goto error2;
 851                         else if (ret == 1)      /* locally consumed */
 852                                 goto next;
 853                 }
 854
 855                 /* Allocate MAD send WR tracking structure */
 856                 mad_send_wr = kmalloc(sizeof *mad_send_wr, GFP_ATOMIC);
 857                 if (!mad_send_wr) {
 858                         printk(KERN_ERR PFX "No memory for "
 859                                "ib_mad_send_wr_private\n");
 860                         ret = -ENOMEM;
 861                         goto error2;
 862                 }
 863
 864                 mad_send_wr->send_wr = *send_wr;
 865                 mad_send_wr->send_wr.sg_list = mad_send_wr->sg_list;
 866                 memcpy(mad_send_wr->sg_list, send_wr->sg_list,
 867                        sizeof *send_wr->sg_list * send_wr->num_sge);
 868                 mad_send_wr->send_wr.next = NULL;
 869                 mad_send_wr->tid = send_wr->wr.ud.mad_hdr->tid;
 870                 mad_send_wr->agent = mad_agent;
 871                 /* Timeout will be updated after send completes */
 872                 mad_send_wr->timeout = msecs_to_jiffies(send_wr->wr.
 873                                                         ud.timeout_ms);
 874                 mad_send_wr->retry = 0;
 875                 /* One reference for each work request to QP + response */
 876                 mad_send_wr->refcount = 1 + (mad_send_wr->timeout > 0);
 877                 mad_send_wr->status = IB_WC_SUCCESS;
 878
 879                 /* Reference MAD agent until send completes */
 880                 atomic_inc(&mad_agent_priv->refcount);
 881                 spin_lock_irqsave(&mad_agent_priv->lock, flags);
 882                 list_add_tail(&mad_send_wr->agent_list,
 883                               &mad_agent_priv->send_list);
 884                 spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
 885
 886                 ret = ib_send_mad(mad_agent_priv, mad_send_wr);
 887                 if (ret) {
 888                         /* Fail send request */
 889                         spin_lock_irqsave(&mad_agent_priv->lock, flags);
 890                         list_del(&mad_send_wr->agent_list);
 891                         spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
 892                         atomic_dec(&mad_agent_priv->refcount);
 893                         goto error2;
 894                 }
 895 next:
 896                 send_wr = next_send_wr;
 897         }
 898         return 0;
 899
 900 error2:
 901         *bad_send_wr = send_wr;
 902 error1:
 903         return ret;
 904 }
 905 EXPORT_SYMBOL(ib_post_send_mad);
 906
 907 /*
 908  * ib_free_recv_mad - Returns data buffers used to receive
 909  *  a MAD to the access layer
 910  */
 911 void ib_free_recv_mad(struct ib_mad_recv_wc *mad_recv_wc)
 912 {
 913         struct ib_mad_recv_buf *entry;
 914         struct ib_mad_private_header *mad_priv_hdr;
 915         struct ib_mad_private *priv;
 916
 917         mad_priv_hdr = container_of(mad_recv_wc,
 918                                     struct ib_mad_private_header,
 919                                     recv_wc);
 920         priv = container_of(mad_priv_hdr, struct ib_mad_private, header);
 921
 922         /*
 923          * Walk receive buffer list associated with this WC
 924          * No need to remove them from list of receive buffers
 925          */
 926         list_for_each_entry(entry, &mad_recv_wc->recv_buf.list, list) {
 927                 /* Free previous receive buffer */
 928                 kmem_cache_free(ib_mad_cache, priv);
 929                 mad_priv_hdr = container_of(mad_recv_wc,
 930                                             struct ib_mad_private_header,
 931                                             recv_wc);
 932                 priv = container_of(mad_priv_hdr, struct ib_mad_private,
 933                                     header);
 934         }
 935
 936         /* Free last buffer */
 937         kmem_cache_free(ib_mad_cache, priv);
 938 }
 939 EXPORT_SYMBOL(ib_free_recv_mad);
 940
 941 void ib_coalesce_recv_mad(struct ib_mad_recv_wc *mad_recv_wc,
 942                           void *buf)
 943 {
 944         printk(KERN_ERR PFX "ib_coalesce_recv_mad() not implemented yet\n");
 945 }
 946 EXPORT_SYMBOL(ib_coalesce_recv_mad);
 947
 948 struct ib_mad_agent *ib_redirect_mad_qp(struct ib_qp *qp,
 949                                         u8 rmpp_version,
 950                                         ib_mad_send_handler send_handler,
 951                                         ib_mad_recv_handler recv_handler,
 952                                         void *context)
 953 {
 954         return ERR_PTR(-EINVAL);        /* XXX: for now */
 955 }
 956 EXPORT_SYMBOL(ib_redirect_mad_qp);
 957
 958 int ib_process_mad_wc(struct ib_mad_agent *mad_agent,
 959                       struct ib_wc *wc)
 960 {
 961         printk(KERN_ERR PFX "ib_process_mad_wc() not implemented yet\n");
 962         return 0;
 963 }
 964 EXPORT_SYMBOL(ib_process_mad_wc);
 965
 966 static int method_in_use(struct ib_mad_mgmt_method_table **method,
 967                          struct ib_mad_reg_req *mad_reg_req)
 968 {
 969         int i;
 970
 971         for (i = find_first_bit(mad_reg_req->method_mask, IB_MGMT_MAX_METHODS);
 972              i < IB_MGMT_MAX_METHODS;
 973              i = find_next_bit(mad_reg_req->method_mask, IB_MGMT_MAX_METHODS,
 974                                1+i)) {
 975                 if ((*method)->agent[i]) {
 976                         printk(KERN_ERR PFX "Method %d already in use\n", i);
 977                         return -EINVAL;
 978                 }
 979         }
 980         return 0;
 981 }
 982
 983 static int allocate_method_table(struct ib_mad_mgmt_method_table **method)
 984 {
 985         /* Allocate management method table */
 986         *method = kmalloc(sizeof **method, GFP_ATOMIC);
 987         if (!*method) {
 988                 printk(KERN_ERR PFX "No memory for "
 989                        "ib_mad_mgmt_method_table\n");
 990                 return -ENOMEM;
 991         }
 992         /* Clear management method table */
 993         memset(*method, 0, sizeof **method);
 994
 995         return 0;
 996 }
 997
 998 /*
 999  * Check to see if there are any methods still in use
1000  */
1001 static int check_method_table(struct ib_mad_mgmt_method_table *method)
1002 {
1003         int i;
1004
1005         for (i = 0; i < IB_MGMT_MAX_METHODS; i++)
1006                 if (method->agent[i])
1007                         return 1;
1008         return 0;
1009 }
1010
1011 /*
1012  * Check to see if there are any method tables for this class still in use
1013  */
1014 static int check_class_table(struct ib_mad_mgmt_class_table *class)
1015 {
1016         int i;
1017
1018         for (i = 0; i < MAX_MGMT_CLASS; i++)
1019                 if (class->method_table[i])
1020                         return 1;
1021         return 0;
1022 }
1023
1024 static int check_vendor_class(struct ib_mad_mgmt_vendor_class *vendor_class)
1025 {
1026         int i;
1027
1028         for (i = 0; i < MAX_MGMT_OUI; i++)
1029                 if (vendor_class->method_table[i])
1030                         return 1;
1031         return 0;
1032 }
1033
1034 static int find_vendor_oui(struct ib_mad_mgmt_vendor_class *vendor_class,
1035                            char *oui)
1036 {
1037         int i;
1038
1039         for (i = 0; i < MAX_MGMT_OUI; i++)
1040                 /* Is there matching OUI for this vendor class ? */
1041                 if (!memcmp(vendor_class->oui[i], oui, 3))
1042                         return i;
1043
1044         return -1;
1045 }
1046
1047 static int check_vendor_table(struct ib_mad_mgmt_vendor_class_table *vendor)
1048 {
1049         int i;
1050
1051         for (i = 0; i < MAX_MGMT_VENDOR_RANGE2; i++)
1052                 if (vendor->vendor_class[i])
1053                         return 1;
1054
1055         return 0;
1056 }
1057
1058 static void remove_methods_mad_agent(struct ib_mad_mgmt_method_table *method,
1059                                      struct ib_mad_agent_private *agent)
1060 {
1061         int i;
1062
1063         /* Remove any methods for this mad agent */
1064         for (i = 0; i < IB_MGMT_MAX_METHODS; i++) {
1065                 if (method->agent[i] == agent) {
1066                         method->agent[i] = NULL;
1067                 }
1068         }
1069 }
1070
1071 static int add_nonoui_reg_req(struct ib_mad_reg_req *mad_reg_req,
1072                               struct ib_mad_agent_private *agent_priv,
1073                               u8 mgmt_class)
1074 {
1075         struct ib_mad_port_private *port_priv;
1076         struct ib_mad_mgmt_class_table **class;
1077         struct ib_mad_mgmt_method_table **method;
1078         int i, ret;
1079
1080         port_priv = agent_priv->qp_info->port_priv;
1081         class = &port_priv->version[mad_reg_req->mgmt_class_version].class;
1082         if (!*class) {
1083                 /* Allocate management class table for "new" class version */
1084                 *class = kmalloc(sizeof **class, GFP_ATOMIC);
1085                 if (!*class) {
1086                         printk(KERN_ERR PFX "No memory for "
1087                                "ib_mad_mgmt_class_table\n");
1088                         ret = -ENOMEM;
1089                         goto error1;
1090                 }
1091                 /* Clear management class table */
1092                 memset(*class, 0, sizeof(**class));
1093                 /* Allocate method table for this management class */
1094                 method = &(*class)->method_table[mgmt_class];
1095                 if ((ret = allocate_method_table(method)))
1096                         goto error2;
1097         } else {
1098                 method = &(*class)->method_table[mgmt_class];
1099                 if (!*method) {
1100                         /* Allocate method table for this management class */
1101                         if ((ret = allocate_method_table(method)))
1102                                 goto error1;
1103                 }
1104         }
1105
1106         /* Now, make sure methods are not already in use */
1107         if (method_in_use(method, mad_reg_req))
1108                 goto error3;
1109
1110         /* Finally, add in methods being registered */
1111         for (i = find_first_bit(mad_reg_req->method_mask,
1112                                 IB_MGMT_MAX_METHODS);
1113              i < IB_MGMT_MAX_METHODS;
1114              i = find_next_bit(mad_reg_req->method_mask, IB_MGMT_MAX_METHODS,
1115                                1+i)) {
1116                 (*method)->agent[i] = agent_priv;
1117         }
1118         return 0;
1119
1120 error3:
1121         /* Remove any methods for this mad agent */
1122         remove_methods_mad_agent(*method, agent_priv);
1123         /* Now, check to see if there are any methods in use */
1124         if (!check_method_table(*method)) {
1125                 /* If not, release management method table */
1126                 kfree(*method);
1127                 *method = NULL;
1128         }
1129         ret = -EINVAL;
1130         goto error1;
1131 error2:
1132         kfree(*class);
1133         *class = NULL;
1134 error1:
1135         return ret;
1136 }
1137
1138 static int add_oui_reg_req(struct ib_mad_reg_req *mad_reg_req,
1139                            struct ib_mad_agent_private *agent_priv)
1140 {
1141         struct ib_mad_port_private *port_priv;
1142         struct ib_mad_mgmt_vendor_class_table **vendor_table;
1143         struct ib_mad_mgmt_vendor_class_table *vendor = NULL;
1144         struct ib_mad_mgmt_vendor_class *vendor_class = NULL;
1145         struct ib_mad_mgmt_method_table **method;
1146         int i, ret = -ENOMEM;
1147         u8 vclass;
1148
1149         /* "New" vendor (with OUI) class */
1150         vclass = vendor_class_index(mad_reg_req->mgmt_class);
1151         port_priv = agent_priv->qp_info->port_priv;
1152         vendor_table = &port_priv->version[
1153                                 mad_reg_req->mgmt_class_version].vendor;
1154         if (!*vendor_table) {
1155                 /* Allocate mgmt vendor class table for "new" class version */
1156                 vendor = kmalloc(sizeof *vendor, GFP_ATOMIC);
1157                 if (!vendor) {
1158                         printk(KERN_ERR PFX "No memory for "
1159                                "ib_mad_mgmt_vendor_class_table\n");
1160                         goto error1;
1161                 }
1162                 /* Clear management vendor class table */
1163                 memset(vendor, 0, sizeof(*vendor));
1164                 *vendor_table = vendor;
1165         }
1166         if (!(*vendor_table)->vendor_class[vclass]) {
1167                 /* Allocate table for this management vendor class */
1168                 vendor_class = kmalloc(sizeof *vendor_class, GFP_ATOMIC);
1169                 if (!vendor_class) {
1170                         printk(KERN_ERR PFX "No memory for "
1171                                "ib_mad_mgmt_vendor_class\n");
1172                         goto error2;
1173                 }
1174                 memset(vendor_class, 0, sizeof(*vendor_class));
1175                 (*vendor_table)->vendor_class[vclass] = vendor_class;
1176         }
1177         for (i = 0; i < MAX_MGMT_OUI; i++) {
1178                 /* Is there matching OUI for this vendor class ? */
1179                 if (!memcmp((*vendor_table)->vendor_class[vclass]->oui[i],
1180                             mad_reg_req->oui, 3)) {
1181                         method = &(*vendor_table)->vendor_class[
1182                                                 vclass]->method_table[i];
1183                         BUG_ON(!*method);
1184                         goto check_in_use;
1185                 }
1186         }
1187         for (i = 0; i < MAX_MGMT_OUI; i++) {
1188                 /* OUI slot available ? */
1189                 if (!is_vendor_oui((*vendor_table)->vendor_class[
1190                                 vclass]->oui[i])) {
1191                         method = &(*vendor_table)->vendor_class[
1192                                 vclass]->method_table[i];
1193                         BUG_ON(*method);
1194                         /* Allocate method table for this OUI */
1195                         if ((ret = allocate_method_table(method)))
1196                                 goto error3;
1197                         memcpy((*vendor_table)->vendor_class[vclass]->oui[i],
1198                                mad_reg_req->oui, 3);
1199                         goto check_in_use;
1200                 }
1201         }
1202         printk(KERN_ERR PFX "All OUI slots in use\n");
1203         goto error3;
1204
1205 check_in_use:
1206         /* Now, make sure methods are not already in use */
1207         if (method_in_use(method, mad_reg_req))
1208                 goto error4;
1209
1210         /* Finally, add in methods being registered */
1211         for (i = find_first_bit(mad_reg_req->method_mask,
1212                                 IB_MGMT_MAX_METHODS);
1213              i < IB_MGMT_MAX_METHODS;
1214              i = find_next_bit(mad_reg_req->method_mask, IB_MGMT_MAX_METHODS,
1215                                1+i)) {
1216                 (*method)->agent[i] = agent_priv;
1217         }
1218         return 0;
1219
1220 error4:
1221         /* Remove any methods for this mad agent */
1222         remove_methods_mad_agent(*method, agent_priv);
1223         /* Now, check to see if there are any methods in use */
1224         if (!check_method_table(*method)) {
1225                 /* If not, release management method table */
1226                 kfree(*method);
1227                 *method = NULL;
1228         }
1229         ret = -EINVAL;
1230 error3:
1231         if (vendor_class) {
1232                 (*vendor_table)->vendor_class[vclass] = NULL;
1233                 kfree(vendor_class);
1234         }
1235 error2:
1236         if (vendor) {
1237                 *vendor_table = NULL;
1238                 kfree(vendor);
1239         }
1240 error1:
1241         return ret;
1242 }
1243
1244 static void remove_mad_reg_req(struct ib_mad_agent_private *agent_priv)
1245 {
1246         struct ib_mad_port_private *port_priv;
1247         struct ib_mad_mgmt_class_table *class;
1248         struct ib_mad_mgmt_method_table *method;
1249         struct ib_mad_mgmt_vendor_class_table *vendor;
1250         struct ib_mad_mgmt_vendor_class *vendor_class;
1251         int index;
1252         u8 mgmt_class;
1253
1254         /*
1255          * Was MAD registration request supplied
1256          * with original registration ?
1257          */
1258         if (!agent_priv->reg_req) {
1259                 goto out;
1260         }
1261
1262         port_priv = agent_priv->qp_info->port_priv;
1263         mgmt_class = convert_mgmt_class(agent_priv->reg_req->mgmt_class);
1264         class = port_priv->version[
1265                         agent_priv->reg_req->mgmt_class_version].class;
1266         if (!class)
1267                 goto vendor_check;
1268
1269         method = class->method_table[mgmt_class];
1270         if (method) {
1271                 /* Remove any methods for this mad agent */
1272                 remove_methods_mad_agent(method, agent_priv);
1273                 /* Now, check to see if there are any methods still in use */
1274                 if (!check_method_table(method)) {
1275                         /* If not, release management method table */
1276                          kfree(method);
1277                          class->method_table[mgmt_class] = NULL;
1278                          /* Any management classes left ? */
1279                         if (!check_class_table(class)) {
1280                                 /* If not, release management class table */
1281                                 kfree(class);
1282                                 port_priv->version[
1283                                         agent_priv->reg_req->
1284                                         mgmt_class_version].class = NULL;
1285                         }
1286                 }
1287         }
1288
1289 vendor_check:
1290         if (!is_vendor_class(mgmt_class))
1291                 goto out;
1292
1293         /* normalize mgmt_class to vendor range 2 */
1294         mgmt_class = vendor_class_index(agent_priv->reg_req->mgmt_class);
1295         vendor = port_priv->version[
1296                         agent_priv->reg_req->mgmt_class_version].vendor;
1297
1298         if (!vendor)
1299                 goto out;
1300
1301         vendor_class = vendor->vendor_class[mgmt_class];
1302         if (vendor_class) {
1303                 index = find_vendor_oui(vendor_class, agent_priv->reg_req->oui);
1304                 if (index < 0)
1305                         goto out;
1306                 method = vendor_class->method_table[index];
1307                 if (method) {
1308                         /* Remove any methods for this mad agent */
1309                         remove_methods_mad_agent(method, agent_priv);
1310                         /*
1311                          * Now, check to see if there are
1312                          * any methods still in use
1313                          */
1314                         if (!check_method_table(method)) {
1315                                 /* If not, release management method table */
1316                                 kfree(method);
1317                                 vendor_class->method_table[index] = NULL;
1318                                 memset(vendor_class->oui[index], 0, 3);
1319                                 /* Any OUIs left ? */
1320                                 if (!check_vendor_class(vendor_class)) {
1321                                         /* If not, release vendor class table */
1322                                         kfree(vendor_class);
1323                                         vendor->vendor_class[mgmt_class] = NULL;
1324                                         /* Any other vendor classes left ? */
1325                                         if (!check_vendor_table(vendor)) {
1326                                                 kfree(vendor);
1327                                                 port_priv->version[
1328                                                         agent_priv->reg_req->
1329                                                         mgmt_class_version].
1330                                                         vendor = NULL;
1331                                         }
1332                                 }
1333                         }
1334                 }
1335         }
1336
1337 out:
1338         return;
1339 }
1340
1341 static int response_mad(struct ib_mad *mad)
1342 {
1343         /* Trap represses are responses although response bit is reset */
1344         return ((mad->mad_hdr.method == IB_MGMT_METHOD_TRAP_REPRESS) ||
1345                 (mad->mad_hdr.method & IB_MGMT_METHOD_RESP));
1346 }
1347
1348 static int solicited_mad(struct ib_mad *mad)
1349 {
1350         /* CM MADs are never solicited */
1351         if (mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_CM) {
1352                 return 0;
1353         }
1354
1355         /* XXX: Determine whether MAD is using RMPP */
1356
1357         /* Not using RMPP */
1358         /* Is this MAD a response to a previous MAD ? */
1359         return response_mad(mad);
1360 }
1361
1362 static struct ib_mad_agent_private *
1363 find_mad_agent(struct ib_mad_port_private *port_priv,
1364                struct ib_mad *mad,
1365                int solicited)
1366 {
1367         struct ib_mad_agent_private *mad_agent = NULL;
1368         unsigned long flags;
1369
1370         spin_lock_irqsave(&port_priv->reg_lock, flags);
1371
1372         /*
1373          * Whether MAD was solicited determines type of routing to
1374          * MAD client.
1375          */
1376         if (solicited) {
1377                 u32 hi_tid;
1378                 struct ib_mad_agent_private *entry;
1379
1380                 /*
1381                  * Routing is based on high 32 bits of transaction ID
1382                  * of MAD.
1383                  */
1384                 hi_tid = be64_to_cpu(mad->mad_hdr.tid) >> 32;
1385                 list_for_each_entry(entry, &port_priv->agent_list,
1386                                     agent_list) {
1387                         if (entry->agent.hi_tid == hi_tid) {
1388                                 mad_agent = entry;
1389                                 break;
1390                         }
1391                 }
1392         } else {
1393                 struct ib_mad_mgmt_class_table *class;
1394                 struct ib_mad_mgmt_method_table *method;
1395                 struct ib_mad_mgmt_vendor_class_table *vendor;
1396                 struct ib_mad_mgmt_vendor_class *vendor_class;
1397                 struct ib_vendor_mad *vendor_mad;
1398                 int index;
1399
1400                 /*
1401                  * Routing is based on version, class, and method
1402                  * For "newer" vendor MADs, also based on OUI
1403                  */
1404                 if (mad->mad_hdr.class_version >= MAX_MGMT_VERSION)
1405                         goto out;
1406                 if (!is_vendor_class(mad->mad_hdr.mgmt_class)) {
1407                         class = port_priv->version[
1408                                         mad->mad_hdr.class_version].class;
1409                         if (!class)
1410                                 goto out;
1411                         method = class->method_table[convert_mgmt_class(
1412                                                         mad->mad_hdr.mgmt_class)];
1413                         if (method)
1414                                 mad_agent = method->agent[mad->mad_hdr.method &
1415                                                           ~IB_MGMT_METHOD_RESP];
1416                 } else {
1417                         vendor = port_priv->version[
1418                                         mad->mad_hdr.class_version].vendor;
1419                         if (!vendor)
1420                                 goto out;
1421                         vendor_class = vendor->vendor_class[vendor_class_index(
1422                                                 mad->mad_hdr.mgmt_class)];
1423                         if (!vendor_class)
1424                                 goto out;
1425                         /* Find matching OUI */
1426                         vendor_mad = (struct ib_vendor_mad *)mad;
1427                         index = find_vendor_oui(vendor_class, vendor_mad->oui);
1428                         if (index == -1)
1429                                 goto out;
1430                         method = vendor_class->method_table[index];
1431                         if (method) {
1432                                 mad_agent = method->agent[mad->mad_hdr.method &
1433                                                           ~IB_MGMT_METHOD_RESP];
1434                         }
1435                 }
1436         }
1437
1438         if (mad_agent) {
1439                 if (mad_agent->agent.recv_handler)
1440                         atomic_inc(&mad_agent->refcount);
1441                 else {
1442                         printk(KERN_NOTICE PFX "No receive handler for client "
1443                                "%p on port %d\n",
1444                                &mad_agent->agent, port_priv->port_num);
1445                         mad_agent = NULL;
1446                 }
1447         }
1448 out:
1449         spin_unlock_irqrestore(&port_priv->reg_lock, flags);
1450
1451         return mad_agent;
1452 }
1453
1454 static int validate_mad(struct ib_mad *mad, u32 qp_num)
1455 {
1456         int valid = 0;
1457
1458         /* Make sure MAD base version is understood */
1459         if (mad->mad_hdr.base_version != IB_MGMT_BASE_VERSION) {
1460                 printk(KERN_ERR PFX "MAD received with unsupported base "
1461                        "version %d\n", mad->mad_hdr.base_version);
1462                 goto out;
1463         }
1464
1465         /* Filter SMI packets sent to other than QP0 */
1466         if ((mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_SUBN_LID_ROUTED) ||
1467             (mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE)) {
1468                 if (qp_num == 0)
1469                         valid = 1;
1470         } else {
1471                 /* Filter GSI packets sent to QP0 */
1472                 if (qp_num != 0)
1473                         valid = 1;
1474         }
1475
1476 out:
1477         return valid;
1478 }
1479
1480 /*
1481  * Return start of fully reassembled MAD, or NULL, if MAD isn't assembled yet
1482  */
1483 static struct ib_mad_private *
1484 reassemble_recv(struct ib_mad_agent_private *mad_agent_priv,
1485                 struct ib_mad_private *recv)
1486 {
1487         /* Until we have RMPP, all receives are reassembled!... */
1488         INIT_LIST_HEAD(&recv->header.recv_wc.recv_buf.list);
1489         return recv;
1490 }
1491
1492 static struct ib_mad_send_wr_private*
1493 find_send_req(struct ib_mad_agent_private *mad_agent_priv,
1494               u64 tid)
1495 {
1496         struct ib_mad_send_wr_private *mad_send_wr;
1497
1498         list_for_each_entry(mad_send_wr, &mad_agent_priv->wait_list,
1499                             agent_list) {
1500                 if (mad_send_wr->tid == tid)
1501                         return mad_send_wr;
1502         }
1503
1504         /*
1505          * It's possible to receive the response before we've
1506          * been notified that the send has completed
1507          */
1508         list_for_each_entry(mad_send_wr, &mad_agent_priv->send_list,
1509                             agent_list) {
1510                 if (mad_send_wr->tid == tid && mad_send_wr->timeout) {
1511                         /* Verify request has not been canceled */
1512                         return (mad_send_wr->status == IB_WC_SUCCESS) ?
1513                                 mad_send_wr : NULL;
1514                 }
1515         }
1516         return NULL;
1517 }
1518
1519 static void ib_mad_complete_recv(struct ib_mad_agent_private *mad_agent_priv,
1520                                  struct ib_mad_private *recv,
1521                                  int solicited)
1522 {
1523         struct ib_mad_send_wr_private *mad_send_wr;
1524         struct ib_mad_send_wc mad_send_wc;
1525         unsigned long flags;
1526
1527         /* Fully reassemble receive before processing */
1528         recv = reassemble_recv(mad_agent_priv, recv);
1529         if (!recv) {
1530                 if (atomic_dec_and_test(&mad_agent_priv->refcount))
1531                         wake_up(&mad_agent_priv->wait);
1532                 return;
1533         }
1534
1535         /* Complete corresponding request */
1536         if (solicited) {
1537                 spin_lock_irqsave(&mad_agent_priv->lock, flags);
1538                 mad_send_wr = find_send_req(mad_agent_priv,
1539                                             recv->mad.mad.mad_hdr.tid);
1540                 if (!mad_send_wr) {
1541                         spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
1542                         ib_free_recv_mad(&recv->header.recv_wc);
1543                         if (atomic_dec_and_test(&mad_agent_priv->refcount))
1544                                 wake_up(&mad_agent_priv->wait);
1545                         return;
1546                 }
1547                 /* Timeout = 0 means that we won't wait for a response */
1548                 mad_send_wr->timeout = 0;
1549                 spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
1550
1551                 /* Defined behavior is to complete response before request */
1552                 recv->header.recv_wc.wc->wr_id = mad_send_wr->wr_id;
1553                 mad_agent_priv->agent.recv_handler(
1554                                                 &mad_agent_priv->agent,
1555                                                 &recv->header.recv_wc);
1556                 atomic_dec(&mad_agent_priv->refcount);
1557
1558                 mad_send_wc.status = IB_WC_SUCCESS;
1559                 mad_send_wc.vendor_err = 0;
1560                 mad_send_wc.wr_id = mad_send_wr->wr_id;
1561                 ib_mad_complete_send_wr(mad_send_wr, &mad_send_wc);
1562         } else {
1563                 mad_agent_priv->agent.recv_handler(
1564                                                 &mad_agent_priv->agent,
1565                                                 &recv->header.recv_wc);
1566                 if (atomic_dec_and_test(&mad_agent_priv->refcount))
1567                         wake_up(&mad_agent_priv->wait);
1568         }
1569 }
1570
1571 static void ib_mad_recv_done_handler(struct ib_mad_port_private *port_priv,
1572                                      struct ib_wc *wc)
1573 {
1574         struct ib_mad_qp_info *qp_info;
1575         struct ib_mad_private_header *mad_priv_hdr;
1576         struct ib_mad_private *recv, *response;
1577         struct ib_mad_list_head *mad_list;
1578         struct ib_mad_agent_private *mad_agent;
1579         int solicited;
1580
1581         response = kmem_cache_alloc(ib_mad_cache, GFP_KERNEL);
1582         if (!response)
1583                 printk(KERN_ERR PFX "ib_mad_recv_done_handler no memory "
1584                        "for response buffer\n");
1585
1586         mad_list = (struct ib_mad_list_head *)(unsigned long)wc->wr_id;
1587         qp_info = mad_list->mad_queue->qp_info;
1588         dequeue_mad(mad_list);
1589
1590         mad_priv_hdr = container_of(mad_list, struct ib_mad_private_header,
1591                                     mad_list);
1592         recv = container_of(mad_priv_hdr, struct ib_mad_private, header);
1593         dma_unmap_single(port_priv->device->dma_device,
1594                          pci_unmap_addr(&recv->header, mapping),
1595                          sizeof(struct ib_mad_private) -
1596                          sizeof(struct ib_mad_private_header),
1597                          DMA_FROM_DEVICE);
1598
1599         /* Setup MAD receive work completion from "normal" work completion */
1600         recv->header.wc = *wc;
1601         recv->header.recv_wc.wc = &recv->header.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 cancel_sends(void *data)
2003 {
2004         struct ib_mad_agent_private *mad_agent_priv;
2005         struct ib_mad_send_wr_private *mad_send_wr;
2006         struct ib_mad_send_wc mad_send_wc;
2007         unsigned long flags;
2008
2009         mad_agent_priv = data;
2010
2011         mad_send_wc.status = IB_WC_WR_FLUSH_ERR;
2012         mad_send_wc.vendor_err = 0;
2013
2014         spin_lock_irqsave(&mad_agent_priv->lock, flags);
2015         while (!list_empty(&mad_agent_priv->canceled_list)) {
2016                 mad_send_wr = list_entry(mad_agent_priv->canceled_list.next,
2017                                          struct ib_mad_send_wr_private,
2018                                          agent_list);
2019
2020                 list_del(&mad_send_wr->agent_list);
2021                 spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
2022
2023                 mad_send_wc.wr_id = mad_send_wr->wr_id;
2024                 mad_agent_priv->agent.send_handler(&mad_agent_priv->agent,
2025                                                    &mad_send_wc);
2026
2027                 kfree(mad_send_wr);
2028                 if (atomic_dec_and_test(&mad_agent_priv->refcount))
2029                         wake_up(&mad_agent_priv->wait);
2030                 spin_lock_irqsave(&mad_agent_priv->lock, flags);
2031         }
2032         spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
2033 }
2034
2035 void ib_cancel_mad(struct ib_mad_agent *mad_agent,
2036                   u64 wr_id)
2037 {
2038         struct ib_mad_agent_private *mad_agent_priv;
2039         struct ib_mad_send_wr_private *mad_send_wr;
2040         unsigned long flags;
2041
2042         mad_agent_priv = container_of(mad_agent, struct ib_mad_agent_private,
2043                                       agent);
2044         spin_lock_irqsave(&mad_agent_priv->lock, flags);
2045         mad_send_wr = find_send_by_wr_id(mad_agent_priv, wr_id);
2046         if (!mad_send_wr) {
2047                 spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
2048                 goto out;
2049         }
2050
2051         if (mad_send_wr->status == IB_WC_SUCCESS)
2052                 mad_send_wr->refcount -= (mad_send_wr->timeout > 0);
2053
2054         if (mad_send_wr->refcount != 0) {
2055                 mad_send_wr->status = IB_WC_WR_FLUSH_ERR;
2056                 spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
2057                 goto out;
2058         }
2059
2060         list_del(&mad_send_wr->agent_list);
2061         list_add_tail(&mad_send_wr->agent_list, &mad_agent_priv->canceled_list);
2062         adjust_timeout(mad_agent_priv);
2063         spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
2064
2065         queue_work(mad_agent_priv->qp_info->port_priv->wq,
2066                    &mad_agent_priv->canceled_work);
2067 out:
2068         return;
2069 }
2070 EXPORT_SYMBOL(ib_cancel_mad);
2071
2072 static void local_completions(void *data)
2073 {
2074         struct ib_mad_agent_private *mad_agent_priv;
2075         struct ib_mad_local_private *local;
2076         struct ib_mad_agent_private *recv_mad_agent;
2077         unsigned long flags;
2078         struct ib_wc wc;
2079         struct ib_mad_send_wc mad_send_wc;
2080
2081         mad_agent_priv = (struct ib_mad_agent_private *)data;
2082
2083         spin_lock_irqsave(&mad_agent_priv->lock, flags);
2084         while (!list_empty(&mad_agent_priv->local_list)) {
2085                 local = list_entry(mad_agent_priv->local_list.next,
2086                                    struct ib_mad_local_private,
2087                                    completion_list);
2088                 spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
2089                 if (local->mad_priv) {
2090                         recv_mad_agent = local->recv_mad_agent;
2091                         if (!recv_mad_agent) {
2092                                 printk(KERN_ERR PFX "No receive MAD agent for local completion\n");
2093                                 kmem_cache_free(ib_mad_cache, local->mad_priv);
2094                                 goto local_send_completion;
2095                         }
2096
2097                         /*
2098                          * Defined behavior is to complete response
2099                          * before request
2100                          */
2101                         build_smp_wc(local->wr_id, IB_LID_PERMISSIVE,
2102                                      0 /* pkey index */,
2103                                      recv_mad_agent->agent.port_num, &wc);
2104
2105                         local->mad_priv->header.recv_wc.wc = &wc;
2106                         local->mad_priv->header.recv_wc.mad_len =
2107                                                 sizeof(struct ib_mad);
2108                         INIT_LIST_HEAD(&local->mad_priv->header.recv_wc.recv_buf.list);
2109                         local->mad_priv->header.recv_wc.recv_buf.grh = NULL;
2110                         local->mad_priv->header.recv_wc.recv_buf.mad =
2111                                                 &local->mad_priv->mad.mad;
2112                         if (atomic_read(&recv_mad_agent->qp_info->snoop_count))
2113                                 snoop_recv(recv_mad_agent->qp_info,
2114                                           &local->mad_priv->header.recv_wc,
2115                                            IB_MAD_SNOOP_RECVS);
2116                         recv_mad_agent->agent.recv_handler(
2117                                                 &recv_mad_agent->agent,
2118                                                 &local->mad_priv->header.recv_wc);
2119                         spin_lock_irqsave(&recv_mad_agent->lock, flags);
2120                         atomic_dec(&recv_mad_agent->refcount);
2121                         spin_unlock_irqrestore(&recv_mad_agent->lock, flags);
2122                 }
2123
2124 local_send_completion:
2125                 /* Complete send */
2126                 mad_send_wc.status = IB_WC_SUCCESS;
2127                 mad_send_wc.vendor_err = 0;
2128                 mad_send_wc.wr_id = local->wr_id;
2129                 if (atomic_read(&mad_agent_priv->qp_info->snoop_count))
2130                         snoop_send(mad_agent_priv->qp_info, &local->send_wr,
2131                                   &mad_send_wc,
2132                                    IB_MAD_SNOOP_SEND_COMPLETIONS);
2133                 mad_agent_priv->agent.send_handler(&mad_agent_priv->agent,
2134                                                    &mad_send_wc);
2135
2136                 spin_lock_irqsave(&mad_agent_priv->lock, flags);
2137                 list_del(&local->completion_list);
2138                 atomic_dec(&mad_agent_priv->refcount);
2139                 kfree(local);
2140         }
2141         spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
2142 }
2143
2144 static void timeout_sends(void *data)
2145 {
2146         struct ib_mad_agent_private *mad_agent_priv;
2147         struct ib_mad_send_wr_private *mad_send_wr;
2148         struct ib_mad_send_wc mad_send_wc;
2149         unsigned long flags, delay;
2150
2151         mad_agent_priv = (struct ib_mad_agent_private *)data;
2152
2153         mad_send_wc.status = IB_WC_RESP_TIMEOUT_ERR;
2154         mad_send_wc.vendor_err = 0;
2155
2156         spin_lock_irqsave(&mad_agent_priv->lock, flags);
2157         while (!list_empty(&mad_agent_priv->wait_list)) {
2158                 mad_send_wr = list_entry(mad_agent_priv->wait_list.next,
2159                                          struct ib_mad_send_wr_private,
2160                                          agent_list);
2161
2162                 if (time_after(mad_send_wr->timeout, jiffies)) {
2163                         delay = mad_send_wr->timeout - jiffies;
2164                         if ((long)delay <= 0)
2165                                 delay = 1;
2166                         queue_delayed_work(mad_agent_priv->qp_info->
2167                                            port_priv->wq,
2168                                            &mad_agent_priv->timed_work, delay);
2169                         break;
2170                 }
2171
2172                 list_del(&mad_send_wr->agent_list);
2173                 spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
2174
2175                 mad_send_wc.wr_id = mad_send_wr->wr_id;
2176                 mad_agent_priv->agent.send_handler(&mad_agent_priv->agent,
2177                                                    &mad_send_wc);
2178
2179                 kfree(mad_send_wr);
2180                 atomic_dec(&mad_agent_priv->refcount);
2181                 spin_lock_irqsave(&mad_agent_priv->lock, flags);
2182         }
2183         spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
2184 }
2185
2186 static void ib_mad_thread_completion_handler(struct ib_cq *cq)
2187 {
2188         struct ib_mad_port_private *port_priv = cq->cq_context;
2189
2190         queue_work(port_priv->wq, &port_priv->work);
2191 }
2192
2193 /*
2194  * Allocate receive MADs and post receive WRs for them
2195  */
2196 static int ib_mad_post_receive_mads(struct ib_mad_qp_info *qp_info,
2197                                     struct ib_mad_private *mad)
2198 {
2199         unsigned long flags;
2200         int post, ret;
2201         struct ib_mad_private *mad_priv;
2202         struct ib_sge sg_list;
2203         struct ib_recv_wr recv_wr, *bad_recv_wr;
2204         struct ib_mad_queue *recv_queue = &qp_info->recv_queue;
2205
2206         /* Initialize common scatter list fields */
2207         sg_list.length = sizeof *mad_priv - sizeof mad_priv->header;
2208         sg_list.lkey = (*qp_info->port_priv->mr).lkey;
2209
2210         /* Initialize common receive WR fields */
2211         recv_wr.next = NULL;
2212         recv_wr.sg_list = &sg_list;
2213         recv_wr.num_sge = 1;
2214
2215         do {
2216                 /* Allocate and map receive buffer */
2217                 if (mad) {
2218                         mad_priv = mad;
2219                         mad = NULL;
2220                 } else {
2221                         mad_priv = kmem_cache_alloc(ib_mad_cache, GFP_KERNEL);
2222                         if (!mad_priv) {
2223                                 printk(KERN_ERR PFX "No memory for receive buffer\n");
2224                                 ret = -ENOMEM;
2225                                 break;
2226                         }
2227                 }
2228                 sg_list.addr = dma_map_single(qp_info->port_priv->
2229                                                 device->dma_device,
2230                                         &mad_priv->grh,
2231                                         sizeof *mad_priv -
2232                                                 sizeof mad_priv->header,
2233                                         DMA_FROM_DEVICE);
2234                 pci_unmap_addr_set(&mad_priv->header, mapping, sg_list.addr);
2235                 recv_wr.wr_id = (unsigned long)&mad_priv->header.mad_list;
2236                 mad_priv->header.mad_list.mad_queue = recv_queue;
2237
2238                 /* Post receive WR */
2239                 spin_lock_irqsave(&recv_queue->lock, flags);
2240                 post = (++recv_queue->count < recv_queue->max_active);
2241                 list_add_tail(&mad_priv->header.mad_list.list, &recv_queue->list);
2242                 spin_unlock_irqrestore(&recv_queue->lock, flags);
2243                 ret = ib_post_recv(qp_info->qp, &recv_wr, &bad_recv_wr);
2244                 if (ret) {
2245                         spin_lock_irqsave(&recv_queue->lock, flags);
2246                         list_del(&mad_priv->header.mad_list.list);
2247                         recv_queue->count--;
2248                         spin_unlock_irqrestore(&recv_queue->lock, flags);
2249                         dma_unmap_single(qp_info->port_priv->device->dma_device,
2250                                          pci_unmap_addr(&mad_priv->header,
2251                                                         mapping),
2252                                          sizeof *mad_priv -
2253                                            sizeof mad_priv->header,
2254                                          DMA_FROM_DEVICE);
2255                         kmem_cache_free(ib_mad_cache, mad_priv);
2256                         printk(KERN_ERR PFX "ib_post_recv failed: %d\n", ret);
2257                         break;
2258                 }
2259         } while (post);
2260
2261         return ret;
2262 }
2263
2264 /*
2265  * Return all the posted receive MADs
2266  */
2267 static void cleanup_recv_queue(struct ib_mad_qp_info *qp_info)
2268 {
2269         struct ib_mad_private_header *mad_priv_hdr;
2270         struct ib_mad_private *recv;
2271         struct ib_mad_list_head *mad_list;
2272
2273         while (!list_empty(&qp_info->recv_queue.list)) {
2274
2275                 mad_list = list_entry(qp_info->recv_queue.list.next,
2276                                       struct ib_mad_list_head, list);
2277                 mad_priv_hdr = container_of(mad_list,
2278                                             struct ib_mad_private_header,
2279                                             mad_list);
2280                 recv = container_of(mad_priv_hdr, struct ib_mad_private,
2281                                     header);
2282
2283                 /* Remove from posted receive MAD list */
2284                 list_del(&mad_list->list);
2285
2286                 dma_unmap_single(qp_info->port_priv->device->dma_device,
2287                                  pci_unmap_addr(&recv->header, mapping),
2288                                  sizeof(struct ib_mad_private) -
2289                                  sizeof(struct ib_mad_private_header),
2290                                  DMA_FROM_DEVICE);
2291                 kmem_cache_free(ib_mad_cache, recv);
2292         }
2293
2294         qp_info->recv_queue.count = 0;
2295 }
2296
2297 /*
2298  * Start the port
2299  */
2300 static int ib_mad_port_start(struct ib_mad_port_private *port_priv)
2301 {
2302         int ret, i;
2303         struct ib_qp_attr *attr;
2304         struct ib_qp *qp;
2305
2306         attr = kmalloc(sizeof *attr, GFP_KERNEL);
2307         if (!attr) {
2308                 printk(KERN_ERR PFX "Couldn't kmalloc ib_qp_attr\n");
2309                 return -ENOMEM;
2310         }
2311
2312         for (i = 0; i < IB_MAD_QPS_CORE; i++) {
2313                 qp = port_priv->qp_info[i].qp;
2314                 /*
2315                  * PKey index for QP1 is irrelevant but
2316                  * one is needed for the Reset to Init transition
2317                  */
2318                 attr->qp_state = IB_QPS_INIT;
2319                 attr->pkey_index = 0;
2320                 attr->qkey = (qp->qp_num == 0) ? 0 : IB_QP1_QKEY;
2321                 ret = ib_modify_qp(qp, attr, IB_QP_STATE |
2322                                              IB_QP_PKEY_INDEX | IB_QP_QKEY);
2323                 if (ret) {
2324                         printk(KERN_ERR PFX "Couldn't change QP%d state to "
2325                                "INIT: %d\n", i, ret);
2326                         goto out;
2327                 }
2328
2329                 attr->qp_state = IB_QPS_RTR;
2330                 ret = ib_modify_qp(qp, attr, IB_QP_STATE);
2331                 if (ret) {
2332                         printk(KERN_ERR PFX "Couldn't change QP%d state to "
2333                                "RTR: %d\n", i, ret);
2334                         goto out;
2335                 }
2336
2337                 attr->qp_state = IB_QPS_RTS;
2338                 attr->sq_psn = IB_MAD_SEND_Q_PSN;
2339                 ret = ib_modify_qp(qp, attr, IB_QP_STATE | IB_QP_SQ_PSN);
2340                 if (ret) {
2341                         printk(KERN_ERR PFX "Couldn't change QP%d state to "
2342                                "RTS: %d\n", i, ret);
2343                         goto out;
2344                 }
2345         }
2346
2347         ret = ib_req_notify_cq(port_priv->cq, IB_CQ_NEXT_COMP);
2348         if (ret) {
2349                 printk(KERN_ERR PFX "Failed to request completion "
2350                        "notification: %d\n", ret);
2351                 goto out;
2352         }
2353
2354         for (i = 0; i < IB_MAD_QPS_CORE; i++) {
2355                 ret = ib_mad_post_receive_mads(&port_priv->qp_info[i], NULL);
2356                 if (ret) {
2357                         printk(KERN_ERR PFX "Couldn't post receive WRs\n");
2358                         goto out;
2359                 }
2360         }
2361 out:
2362         kfree(attr);
2363         return ret;
2364 }
2365
2366 static void qp_event_handler(struct ib_event *event, void *qp_context)
2367 {
2368         struct ib_mad_qp_info   *qp_info = qp_context;
2369
2370         /* It's worse than that! He's dead, Jim! */
2371         printk(KERN_ERR PFX "Fatal error (%d) on MAD QP (%d)\n",
2372                 event->event, qp_info->qp->qp_num);
2373 }
2374
2375 static void init_mad_queue(struct ib_mad_qp_info *qp_info,
2376                            struct ib_mad_queue *mad_queue)
2377 {
2378         mad_queue->qp_info = qp_info;
2379         mad_queue->count = 0;
2380         spin_lock_init(&mad_queue->lock);
2381         INIT_LIST_HEAD(&mad_queue->list);
2382 }
2383
2384 static void init_mad_qp(struct ib_mad_port_private *port_priv,
2385                         struct ib_mad_qp_info *qp_info)
2386 {
2387         qp_info->port_priv = port_priv;
2388         init_mad_queue(qp_info, &qp_info->send_queue);
2389         init_mad_queue(qp_info, &qp_info->recv_queue);
2390         INIT_LIST_HEAD(&qp_info->overflow_list);
2391         spin_lock_init(&qp_info->snoop_lock);
2392         qp_info->snoop_table = NULL;
2393         qp_info->snoop_table_size = 0;
2394         atomic_set(&qp_info->snoop_count, 0);
2395 }
2396
2397 static int create_mad_qp(struct ib_mad_qp_info *qp_info,
2398                          enum ib_qp_type qp_type)
2399 {
2400         struct ib_qp_init_attr  qp_init_attr;
2401         int ret;
2402
2403         memset(&qp_init_attr, 0, sizeof qp_init_attr);
2404         qp_init_attr.send_cq = qp_info->port_priv->cq;
2405         qp_init_attr.recv_cq = qp_info->port_priv->cq;
2406         qp_init_attr.sq_sig_type = IB_SIGNAL_ALL_WR;
2407         qp_init_attr.cap.max_send_wr = IB_MAD_QP_SEND_SIZE;
2408         qp_init_attr.cap.max_recv_wr = IB_MAD_QP_RECV_SIZE;
2409         qp_init_attr.cap.max_send_sge = IB_MAD_SEND_REQ_MAX_SG;
2410         qp_init_attr.cap.max_recv_sge = IB_MAD_RECV_REQ_MAX_SG;
2411         qp_init_attr.qp_type = qp_type;
2412         qp_init_attr.port_num = qp_info->port_priv->port_num;
2413         qp_init_attr.qp_context = qp_info;
2414         qp_init_attr.event_handler = qp_event_handler;
2415         qp_info->qp = ib_create_qp(qp_info->port_priv->pd, &qp_init_attr);
2416         if (IS_ERR(qp_info->qp)) {
2417                 printk(KERN_ERR PFX "Couldn't create ib_mad QP%d\n",
2418                        get_spl_qp_index(qp_type));
2419                 ret = PTR_ERR(qp_info->qp);
2420                 goto error;
2421         }
2422         /* Use minimum queue sizes unless the CQ is resized */
2423         qp_info->send_queue.max_active = IB_MAD_QP_SEND_SIZE;
2424         qp_info->recv_queue.max_active = IB_MAD_QP_RECV_SIZE;
2425         return 0;
2426
2427 error:
2428         return ret;
2429 }
2430
2431 static void destroy_mad_qp(struct ib_mad_qp_info *qp_info)
2432 {
2433         ib_destroy_qp(qp_info->qp);
2434         if (qp_info->snoop_table)
2435                 kfree(qp_info->snoop_table);
2436 }
2437
2438 /*
2439  * Open the port
2440  * Create the QP, PD, MR, and CQ if needed
2441  */
2442 static int ib_mad_port_open(struct ib_device *device,
2443                             int port_num)
2444 {
2445         int ret, cq_size;
2446         struct ib_mad_port_private *port_priv;
2447         unsigned long flags;
2448         char name[sizeof "ib_mad123"];
2449
2450         /* First, check if port already open at MAD layer */
2451         port_priv = ib_get_mad_port(device, port_num);
2452         if (port_priv) {
2453                 printk(KERN_DEBUG PFX "%s port %d already open\n",
2454                        device->name, port_num);
2455                 return 0;
2456         }
2457
2458         /* Create new device info */
2459         port_priv = kmalloc(sizeof *port_priv, GFP_KERNEL);
2460         if (!port_priv) {
2461                 printk(KERN_ERR PFX "No memory for ib_mad_port_private\n");
2462                 return -ENOMEM;
2463         }
2464         memset(port_priv, 0, sizeof *port_priv);
2465         port_priv->device = device;
2466         port_priv->port_num = port_num;
2467         spin_lock_init(&port_priv->reg_lock);
2468         INIT_LIST_HEAD(&port_priv->agent_list);
2469         init_mad_qp(port_priv, &port_priv->qp_info[0]);
2470         init_mad_qp(port_priv, &port_priv->qp_info[1]);
2471
2472         cq_size = (IB_MAD_QP_SEND_SIZE + IB_MAD_QP_RECV_SIZE) * 2;
2473         port_priv->cq = ib_create_cq(port_priv->device,
2474                                      (ib_comp_handler)
2475                                         ib_mad_thread_completion_handler,
2476                                      NULL, port_priv, cq_size);
2477         if (IS_ERR(port_priv->cq)) {
2478                 printk(KERN_ERR PFX "Couldn't create ib_mad CQ\n");
2479                 ret = PTR_ERR(port_priv->cq);
2480                 goto error3;
2481         }
2482
2483         port_priv->pd = ib_alloc_pd(device);
2484         if (IS_ERR(port_priv->pd)) {
2485                 printk(KERN_ERR PFX "Couldn't create ib_mad PD\n");
2486                 ret = PTR_ERR(port_priv->pd);
2487                 goto error4;
2488         }
2489
2490         port_priv->mr = ib_get_dma_mr(port_priv->pd, IB_ACCESS_LOCAL_WRITE);
2491         if (IS_ERR(port_priv->mr)) {
2492                 printk(KERN_ERR PFX "Couldn't get ib_mad DMA MR\n");
2493                 ret = PTR_ERR(port_priv->mr);
2494                 goto error5;
2495         }
2496
2497         ret = create_mad_qp(&port_priv->qp_info[0], IB_QPT_SMI);
2498         if (ret)
2499                 goto error6;
2500         ret = create_mad_qp(&port_priv->qp_info[1], IB_QPT_GSI);
2501         if (ret)
2502                 goto error7;
2503
2504         snprintf(name, sizeof name, "ib_mad%d", port_num);
2505         port_priv->wq = create_singlethread_workqueue(name);
2506         if (!port_priv->wq) {
2507                 ret = -ENOMEM;
2508                 goto error8;
2509         }
2510         INIT_WORK(&port_priv->work, ib_mad_completion_handler, port_priv);
2511
2512         ret = ib_mad_port_start(port_priv);
2513         if (ret) {
2514                 printk(KERN_ERR PFX "Couldn't start port\n");
2515                 goto error9;
2516         }
2517
2518         spin_lock_irqsave(&ib_mad_port_list_lock, flags);
2519         list_add_tail(&port_priv->port_list, &ib_mad_port_list);
2520         spin_unlock_irqrestore(&ib_mad_port_list_lock, flags);
2521         return 0;
2522
2523 error9:
2524         destroy_workqueue(port_priv->wq);
2525 error8:
2526         destroy_mad_qp(&port_priv->qp_info[1]);
2527 error7:
2528         destroy_mad_qp(&port_priv->qp_info[0]);
2529 error6:
2530         ib_dereg_mr(port_priv->mr);
2531 error5:
2532         ib_dealloc_pd(port_priv->pd);
2533 error4:
2534         ib_destroy_cq(port_priv->cq);
2535         cleanup_recv_queue(&port_priv->qp_info[1]);
2536         cleanup_recv_queue(&port_priv->qp_info[0]);
2537 error3:
2538         kfree(port_priv);
2539
2540         return ret;
2541 }
2542
2543 /*
2544  * Close the port
2545  * If there are no classes using the port, free the port
2546  * resources (CQ, MR, PD, QP) and remove the port's info structure
2547  */
2548 static int ib_mad_port_close(struct ib_device *device, int port_num)
2549 {
2550         struct ib_mad_port_private *port_priv;
2551         unsigned long flags;
2552
2553         spin_lock_irqsave(&ib_mad_port_list_lock, flags);
2554         port_priv = __ib_get_mad_port(device, port_num);
2555         if (port_priv == NULL) {
2556                 spin_unlock_irqrestore(&ib_mad_port_list_lock, flags);
2557                 printk(KERN_ERR PFX "Port %d not found\n", port_num);
2558                 return -ENODEV;
2559         }
2560         list_del(&port_priv->port_list);
2561         spin_unlock_irqrestore(&ib_mad_port_list_lock, flags);
2562
2563         /* Stop processing completions. */
2564         flush_workqueue(port_priv->wq);
2565         destroy_workqueue(port_priv->wq);
2566         destroy_mad_qp(&port_priv->qp_info[1]);
2567         destroy_mad_qp(&port_priv->qp_info[0]);
2568         ib_dereg_mr(port_priv->mr);
2569         ib_dealloc_pd(port_priv->pd);
2570         ib_destroy_cq(port_priv->cq);
2571         cleanup_recv_queue(&port_priv->qp_info[1]);
2572         cleanup_recv_queue(&port_priv->qp_info[0]);
2573         /* XXX: Handle deallocation of MAD registration tables */
2574
2575         kfree(port_priv);
2576
2577         return 0;
2578 }
2579
2580 static void ib_mad_init_device(struct ib_device *device)
2581 {
2582         int ret, num_ports, cur_port, i, ret2;
2583
2584         if (device->node_type == IB_NODE_SWITCH) {
2585                 num_ports = 1;
2586                 cur_port = 0;
2587         } else {
2588                 num_ports = device->phys_port_cnt;
2589                 cur_port = 1;
2590         }
2591         for (i = 0; i < num_ports; i++, cur_port++) {
2592                 ret = ib_mad_port_open(device, cur_port);
2593                 if (ret) {
2594                         printk(KERN_ERR PFX "Couldn't open %s port %d\n",
2595                                device->name, cur_port);
2596                         goto error_device_open;
2597                 }
2598                 ret = ib_agent_port_open(device, cur_port);
2599                 if (ret) {
2600                         printk(KERN_ERR PFX "Couldn't open %s port %d "
2601                                "for agents\n",
2602                                device->name, cur_port);
2603                         goto error_device_open;
2604                 }
2605         }
2606
2607         goto error_device_query;
2608
2609 error_device_open:
2610         while (i > 0) {
2611                 cur_port--;
2612                 ret2 = ib_agent_port_close(device, cur_port);
2613                 if (ret2) {
2614                         printk(KERN_ERR PFX "Couldn't close %s port %d "
2615                                "for agents\n",
2616                                device->name, cur_port);
2617                 }
2618                 ret2 = ib_mad_port_close(device, cur_port);
2619                 if (ret2) {
2620                         printk(KERN_ERR PFX "Couldn't close %s port %d\n",
2621                                device->name, cur_port);
2622                 }
2623                 i--;
2624         }
2625
2626 error_device_query:
2627         return;
2628 }
2629
2630 static void ib_mad_remove_device(struct ib_device *device)
2631 {
2632         int ret = 0, i, num_ports, cur_port, ret2;
2633
2634         if (device->node_type == IB_NODE_SWITCH) {
2635                 num_ports = 1;
2636                 cur_port = 0;
2637         } else {
2638                 num_ports = device->phys_port_cnt;
2639                 cur_port = 1;
2640         }
2641         for (i = 0; i < num_ports; i++, cur_port++) {
2642                 ret2 = ib_agent_port_close(device, cur_port);
2643                 if (ret2) {
2644                         printk(KERN_ERR PFX "Couldn't close %s port %d "
2645                                "for agents\n",
2646                                device->name, cur_port);
2647                         if (!ret)
2648                                 ret = ret2;
2649                 }
2650                 ret2 = ib_mad_port_close(device, cur_port);
2651                 if (ret2) {
2652                         printk(KERN_ERR PFX "Couldn't close %s port %d\n",
2653                                device->name, cur_port);
2654                         if (!ret)
2655                                 ret = ret2;
2656                 }
2657         }
2658 }
2659
2660 static struct ib_client mad_client = {
2661         .name   = "mad",
2662         .add = ib_mad_init_device,
2663         .remove = ib_mad_remove_device
2664 };
2665
2666 static int __init ib_mad_init_module(void)
2667 {
2668         int ret;
2669
2670         spin_lock_init(&ib_mad_port_list_lock);
2671         spin_lock_init(&ib_agent_port_list_lock);
2672
2673         ib_mad_cache = kmem_cache_create("ib_mad",
2674                                          sizeof(struct ib_mad_private),
2675                                          0,
2676                                          SLAB_HWCACHE_ALIGN,
2677                                          NULL,
2678                                          NULL);
2679         if (!ib_mad_cache) {
2680                 printk(KERN_ERR PFX "Couldn't create ib_mad cache\n");
2681                 ret = -ENOMEM;
2682                 goto error1;
2683         }
2684
2685         INIT_LIST_HEAD(&ib_mad_port_list);
2686
2687         if (ib_register_client(&mad_client)) {
2688                 printk(KERN_ERR PFX "Couldn't register ib_mad client\n");
2689                 ret = -EINVAL;
2690                 goto error2;
2691         }
2692
2693         return 0;
2694
2695 error2:
2696         kmem_cache_destroy(ib_mad_cache);
2697 error1:
2698         return ret;
2699 }
2700
2701 static void __exit ib_mad_cleanup_module(void)
2702 {
2703         ib_unregister_client(&mad_client);
2704
2705         if (kmem_cache_destroy(ib_mad_cache)) {
2706                 printk(KERN_DEBUG PFX "Failed to destroy ib_mad cache\n");
2707         }
2708 }
2709
2710 module_init(ib_mad_init_module);
2711 module_exit(ib_mad_cleanup_module);