xref: /openbmc/linux/drivers/infiniband/hw/mlx4/mad.c (revision 9a6b55ac)
1 /*
2  * Copyright (c) 2007 Cisco Systems, 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 
33 #include <rdma/ib_mad.h>
34 #include <rdma/ib_smi.h>
35 #include <rdma/ib_sa.h>
36 #include <rdma/ib_cache.h>
37 
38 #include <linux/random.h>
39 #include <linux/mlx4/cmd.h>
40 #include <linux/gfp.h>
41 #include <rdma/ib_pma.h>
42 #include <linux/ip.h>
43 #include <net/ipv6.h>
44 
45 #include <linux/mlx4/driver.h>
46 #include "mlx4_ib.h"
47 
48 enum {
49 	MLX4_IB_VENDOR_CLASS1 = 0x9,
50 	MLX4_IB_VENDOR_CLASS2 = 0xa
51 };
52 
53 #define MLX4_TUN_SEND_WRID_SHIFT 34
54 #define MLX4_TUN_QPN_SHIFT 32
55 #define MLX4_TUN_WRID_RECV (((u64) 1) << MLX4_TUN_SEND_WRID_SHIFT)
56 #define MLX4_TUN_SET_WRID_QPN(a) (((u64) ((a) & 0x3)) << MLX4_TUN_QPN_SHIFT)
57 
58 #define MLX4_TUN_IS_RECV(a)  (((a) >>  MLX4_TUN_SEND_WRID_SHIFT) & 0x1)
59 #define MLX4_TUN_WRID_QPN(a) (((a) >> MLX4_TUN_QPN_SHIFT) & 0x3)
60 
61  /* Port mgmt change event handling */
62 
63 #define GET_BLK_PTR_FROM_EQE(eqe) be32_to_cpu(eqe->event.port_mgmt_change.params.tbl_change_info.block_ptr)
64 #define GET_MASK_FROM_EQE(eqe) be32_to_cpu(eqe->event.port_mgmt_change.params.tbl_change_info.tbl_entries_mask)
65 #define NUM_IDX_IN_PKEY_TBL_BLK 32
66 #define GUID_TBL_ENTRY_SIZE 8	   /* size in bytes */
67 #define GUID_TBL_BLK_NUM_ENTRIES 8
68 #define GUID_TBL_BLK_SIZE (GUID_TBL_ENTRY_SIZE * GUID_TBL_BLK_NUM_ENTRIES)
69 
70 struct mlx4_mad_rcv_buf {
71 	struct ib_grh grh;
72 	u8 payload[256];
73 } __packed;
74 
75 struct mlx4_mad_snd_buf {
76 	u8 payload[256];
77 } __packed;
78 
79 struct mlx4_tunnel_mad {
80 	struct ib_grh grh;
81 	struct mlx4_ib_tunnel_header hdr;
82 	struct ib_mad mad;
83 } __packed;
84 
85 struct mlx4_rcv_tunnel_mad {
86 	struct mlx4_rcv_tunnel_hdr hdr;
87 	struct ib_grh grh;
88 	struct ib_mad mad;
89 } __packed;
90 
91 static void handle_client_rereg_event(struct mlx4_ib_dev *dev, u8 port_num);
92 static void handle_lid_change_event(struct mlx4_ib_dev *dev, u8 port_num);
93 static void __propagate_pkey_ev(struct mlx4_ib_dev *dev, int port_num,
94 				int block, u32 change_bitmap);
95 
96 __be64 mlx4_ib_gen_node_guid(void)
97 {
98 #define NODE_GUID_HI	((u64) (((u64)IB_OPENIB_OUI) << 40))
99 	return cpu_to_be64(NODE_GUID_HI | prandom_u32());
100 }
101 
102 __be64 mlx4_ib_get_new_demux_tid(struct mlx4_ib_demux_ctx *ctx)
103 {
104 	return cpu_to_be64(atomic_inc_return(&ctx->tid)) |
105 		cpu_to_be64(0xff00000000000000LL);
106 }
107 
108 int mlx4_MAD_IFC(struct mlx4_ib_dev *dev, int mad_ifc_flags,
109 		 int port, const struct ib_wc *in_wc,
110 		 const struct ib_grh *in_grh,
111 		 const void *in_mad, void *response_mad)
112 {
113 	struct mlx4_cmd_mailbox *inmailbox, *outmailbox;
114 	void *inbox;
115 	int err;
116 	u32 in_modifier = port;
117 	u8 op_modifier = 0;
118 
119 	inmailbox = mlx4_alloc_cmd_mailbox(dev->dev);
120 	if (IS_ERR(inmailbox))
121 		return PTR_ERR(inmailbox);
122 	inbox = inmailbox->buf;
123 
124 	outmailbox = mlx4_alloc_cmd_mailbox(dev->dev);
125 	if (IS_ERR(outmailbox)) {
126 		mlx4_free_cmd_mailbox(dev->dev, inmailbox);
127 		return PTR_ERR(outmailbox);
128 	}
129 
130 	memcpy(inbox, in_mad, 256);
131 
132 	/*
133 	 * Key check traps can't be generated unless we have in_wc to
134 	 * tell us where to send the trap.
135 	 */
136 	if ((mad_ifc_flags & MLX4_MAD_IFC_IGNORE_MKEY) || !in_wc)
137 		op_modifier |= 0x1;
138 	if ((mad_ifc_flags & MLX4_MAD_IFC_IGNORE_BKEY) || !in_wc)
139 		op_modifier |= 0x2;
140 	if (mlx4_is_mfunc(dev->dev) &&
141 	    (mad_ifc_flags & MLX4_MAD_IFC_NET_VIEW || in_wc))
142 		op_modifier |= 0x8;
143 
144 	if (in_wc) {
145 		struct {
146 			__be32		my_qpn;
147 			u32		reserved1;
148 			__be32		rqpn;
149 			u8		sl;
150 			u8		g_path;
151 			u16		reserved2[2];
152 			__be16		pkey;
153 			u32		reserved3[11];
154 			u8		grh[40];
155 		} *ext_info;
156 
157 		memset(inbox + 256, 0, 256);
158 		ext_info = inbox + 256;
159 
160 		ext_info->my_qpn = cpu_to_be32(in_wc->qp->qp_num);
161 		ext_info->rqpn   = cpu_to_be32(in_wc->src_qp);
162 		ext_info->sl     = in_wc->sl << 4;
163 		ext_info->g_path = in_wc->dlid_path_bits |
164 			(in_wc->wc_flags & IB_WC_GRH ? 0x80 : 0);
165 		ext_info->pkey   = cpu_to_be16(in_wc->pkey_index);
166 
167 		if (in_grh)
168 			memcpy(ext_info->grh, in_grh, 40);
169 
170 		op_modifier |= 0x4;
171 
172 		in_modifier |= ib_lid_cpu16(in_wc->slid) << 16;
173 	}
174 
175 	err = mlx4_cmd_box(dev->dev, inmailbox->dma, outmailbox->dma, in_modifier,
176 			   mlx4_is_master(dev->dev) ? (op_modifier & ~0x8) : op_modifier,
177 			   MLX4_CMD_MAD_IFC, MLX4_CMD_TIME_CLASS_C,
178 			   (op_modifier & 0x8) ? MLX4_CMD_NATIVE : MLX4_CMD_WRAPPED);
179 
180 	if (!err)
181 		memcpy(response_mad, outmailbox->buf, 256);
182 
183 	mlx4_free_cmd_mailbox(dev->dev, inmailbox);
184 	mlx4_free_cmd_mailbox(dev->dev, outmailbox);
185 
186 	return err;
187 }
188 
189 static void update_sm_ah(struct mlx4_ib_dev *dev, u8 port_num, u16 lid, u8 sl)
190 {
191 	struct ib_ah *new_ah;
192 	struct rdma_ah_attr ah_attr;
193 	unsigned long flags;
194 
195 	if (!dev->send_agent[port_num - 1][0])
196 		return;
197 
198 	memset(&ah_attr, 0, sizeof ah_attr);
199 	ah_attr.type = rdma_ah_find_type(&dev->ib_dev, port_num);
200 	rdma_ah_set_dlid(&ah_attr, lid);
201 	rdma_ah_set_sl(&ah_attr, sl);
202 	rdma_ah_set_port_num(&ah_attr, port_num);
203 
204 	new_ah = rdma_create_ah(dev->send_agent[port_num - 1][0]->qp->pd,
205 				&ah_attr, 0);
206 	if (IS_ERR(new_ah))
207 		return;
208 
209 	spin_lock_irqsave(&dev->sm_lock, flags);
210 	if (dev->sm_ah[port_num - 1])
211 		rdma_destroy_ah(dev->sm_ah[port_num - 1], 0);
212 	dev->sm_ah[port_num - 1] = new_ah;
213 	spin_unlock_irqrestore(&dev->sm_lock, flags);
214 }
215 
216 /*
217  * Snoop SM MADs for port info, GUID info, and  P_Key table sets, so we can
218  * synthesize LID change, Client-Rereg, GID change, and P_Key change events.
219  */
220 static void smp_snoop(struct ib_device *ibdev, u8 port_num, const struct ib_mad *mad,
221 		      u16 prev_lid)
222 {
223 	struct ib_port_info *pinfo;
224 	u16 lid;
225 	__be16 *base;
226 	u32 bn, pkey_change_bitmap;
227 	int i;
228 
229 
230 	struct mlx4_ib_dev *dev = to_mdev(ibdev);
231 	if ((mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_SUBN_LID_ROUTED ||
232 	     mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE) &&
233 	    mad->mad_hdr.method == IB_MGMT_METHOD_SET)
234 		switch (mad->mad_hdr.attr_id) {
235 		case IB_SMP_ATTR_PORT_INFO:
236 			if (dev->dev->caps.flags & MLX4_DEV_CAP_FLAG_PORT_MNG_CHG_EV)
237 				return;
238 			pinfo = (struct ib_port_info *) ((struct ib_smp *) mad)->data;
239 			lid = be16_to_cpu(pinfo->lid);
240 
241 			update_sm_ah(dev, port_num,
242 				     be16_to_cpu(pinfo->sm_lid),
243 				     pinfo->neighbormtu_mastersmsl & 0xf);
244 
245 			if (pinfo->clientrereg_resv_subnetto & 0x80)
246 				handle_client_rereg_event(dev, port_num);
247 
248 			if (prev_lid != lid)
249 				handle_lid_change_event(dev, port_num);
250 			break;
251 
252 		case IB_SMP_ATTR_PKEY_TABLE:
253 			if (dev->dev->caps.flags & MLX4_DEV_CAP_FLAG_PORT_MNG_CHG_EV)
254 				return;
255 			if (!mlx4_is_mfunc(dev->dev)) {
256 				mlx4_ib_dispatch_event(dev, port_num,
257 						       IB_EVENT_PKEY_CHANGE);
258 				break;
259 			}
260 
261 			/* at this point, we are running in the master.
262 			 * Slaves do not receive SMPs.
263 			 */
264 			bn  = be32_to_cpu(((struct ib_smp *)mad)->attr_mod) & 0xFFFF;
265 			base = (__be16 *) &(((struct ib_smp *)mad)->data[0]);
266 			pkey_change_bitmap = 0;
267 			for (i = 0; i < 32; i++) {
268 				pr_debug("PKEY[%d] = x%x\n",
269 					 i + bn*32, be16_to_cpu(base[i]));
270 				if (be16_to_cpu(base[i]) !=
271 				    dev->pkeys.phys_pkey_cache[port_num - 1][i + bn*32]) {
272 					pkey_change_bitmap |= (1 << i);
273 					dev->pkeys.phys_pkey_cache[port_num - 1][i + bn*32] =
274 						be16_to_cpu(base[i]);
275 				}
276 			}
277 			pr_debug("PKEY Change event: port=%d, "
278 				 "block=0x%x, change_bitmap=0x%x\n",
279 				 port_num, bn, pkey_change_bitmap);
280 
281 			if (pkey_change_bitmap) {
282 				mlx4_ib_dispatch_event(dev, port_num,
283 						       IB_EVENT_PKEY_CHANGE);
284 				if (!dev->sriov.is_going_down)
285 					__propagate_pkey_ev(dev, port_num, bn,
286 							    pkey_change_bitmap);
287 			}
288 			break;
289 
290 		case IB_SMP_ATTR_GUID_INFO:
291 			if (dev->dev->caps.flags & MLX4_DEV_CAP_FLAG_PORT_MNG_CHG_EV)
292 				return;
293 			/* paravirtualized master's guid is guid 0 -- does not change */
294 			if (!mlx4_is_master(dev->dev))
295 				mlx4_ib_dispatch_event(dev, port_num,
296 						       IB_EVENT_GID_CHANGE);
297 			/*if master, notify relevant slaves*/
298 			if (mlx4_is_master(dev->dev) &&
299 			    !dev->sriov.is_going_down) {
300 				bn = be32_to_cpu(((struct ib_smp *)mad)->attr_mod);
301 				mlx4_ib_update_cache_on_guid_change(dev, bn, port_num,
302 								    (u8 *)(&((struct ib_smp *)mad)->data));
303 				mlx4_ib_notify_slaves_on_guid_change(dev, bn, port_num,
304 								     (u8 *)(&((struct ib_smp *)mad)->data));
305 			}
306 			break;
307 
308 		case IB_SMP_ATTR_SL_TO_VL_TABLE:
309 			/* cache sl to vl mapping changes for use in
310 			 * filling QP1 LRH VL field when sending packets
311 			 */
312 			if (dev->dev->caps.flags & MLX4_DEV_CAP_FLAG_PORT_MNG_CHG_EV &&
313 			    dev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_SL_TO_VL_CHANGE_EVENT)
314 				return;
315 			if (!mlx4_is_slave(dev->dev)) {
316 				union sl2vl_tbl_to_u64 sl2vl64;
317 				int jj;
318 
319 				for (jj = 0; jj < 8; jj++) {
320 					sl2vl64.sl8[jj] = ((struct ib_smp *)mad)->data[jj];
321 					pr_debug("port %u, sl2vl[%d] = %02x\n",
322 						 port_num, jj, sl2vl64.sl8[jj]);
323 				}
324 				atomic64_set(&dev->sl2vl[port_num - 1], sl2vl64.sl64);
325 			}
326 			break;
327 
328 		default:
329 			break;
330 		}
331 }
332 
333 static void __propagate_pkey_ev(struct mlx4_ib_dev *dev, int port_num,
334 				int block, u32 change_bitmap)
335 {
336 	int i, ix, slave, err;
337 	int have_event = 0;
338 
339 	for (slave = 0; slave < dev->dev->caps.sqp_demux; slave++) {
340 		if (slave == mlx4_master_func_num(dev->dev))
341 			continue;
342 		if (!mlx4_is_slave_active(dev->dev, slave))
343 			continue;
344 
345 		have_event = 0;
346 		for (i = 0; i < 32; i++) {
347 			if (!(change_bitmap & (1 << i)))
348 				continue;
349 			for (ix = 0;
350 			     ix < dev->dev->caps.pkey_table_len[port_num]; ix++) {
351 				if (dev->pkeys.virt2phys_pkey[slave][port_num - 1]
352 				    [ix] == i + 32 * block) {
353 					err = mlx4_gen_pkey_eqe(dev->dev, slave, port_num);
354 					pr_debug("propagate_pkey_ev: slave %d,"
355 						 " port %d, ix %d (%d)\n",
356 						 slave, port_num, ix, err);
357 					have_event = 1;
358 					break;
359 				}
360 			}
361 			if (have_event)
362 				break;
363 		}
364 	}
365 }
366 
367 static void node_desc_override(struct ib_device *dev,
368 			       struct ib_mad *mad)
369 {
370 	unsigned long flags;
371 
372 	if ((mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_SUBN_LID_ROUTED ||
373 	     mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE) &&
374 	    mad->mad_hdr.method == IB_MGMT_METHOD_GET_RESP &&
375 	    mad->mad_hdr.attr_id == IB_SMP_ATTR_NODE_DESC) {
376 		spin_lock_irqsave(&to_mdev(dev)->sm_lock, flags);
377 		memcpy(((struct ib_smp *) mad)->data, dev->node_desc,
378 		       IB_DEVICE_NODE_DESC_MAX);
379 		spin_unlock_irqrestore(&to_mdev(dev)->sm_lock, flags);
380 	}
381 }
382 
383 static void forward_trap(struct mlx4_ib_dev *dev, u8 port_num, const struct ib_mad *mad)
384 {
385 	int qpn = mad->mad_hdr.mgmt_class != IB_MGMT_CLASS_SUBN_LID_ROUTED;
386 	struct ib_mad_send_buf *send_buf;
387 	struct ib_mad_agent *agent = dev->send_agent[port_num - 1][qpn];
388 	int ret;
389 	unsigned long flags;
390 
391 	if (agent) {
392 		send_buf = ib_create_send_mad(agent, qpn, 0, 0, IB_MGMT_MAD_HDR,
393 					      IB_MGMT_MAD_DATA, GFP_ATOMIC,
394 					      IB_MGMT_BASE_VERSION);
395 		if (IS_ERR(send_buf))
396 			return;
397 		/*
398 		 * We rely here on the fact that MLX QPs don't use the
399 		 * address handle after the send is posted (this is
400 		 * wrong following the IB spec strictly, but we know
401 		 * it's OK for our devices).
402 		 */
403 		spin_lock_irqsave(&dev->sm_lock, flags);
404 		memcpy(send_buf->mad, mad, sizeof *mad);
405 		if ((send_buf->ah = dev->sm_ah[port_num - 1]))
406 			ret = ib_post_send_mad(send_buf, NULL);
407 		else
408 			ret = -EINVAL;
409 		spin_unlock_irqrestore(&dev->sm_lock, flags);
410 
411 		if (ret)
412 			ib_free_send_mad(send_buf);
413 	}
414 }
415 
416 static int mlx4_ib_demux_sa_handler(struct ib_device *ibdev, int port, int slave,
417 							     struct ib_sa_mad *sa_mad)
418 {
419 	int ret = 0;
420 
421 	/* dispatch to different sa handlers */
422 	switch (be16_to_cpu(sa_mad->mad_hdr.attr_id)) {
423 	case IB_SA_ATTR_MC_MEMBER_REC:
424 		ret = mlx4_ib_mcg_demux_handler(ibdev, port, slave, sa_mad);
425 		break;
426 	default:
427 		break;
428 	}
429 	return ret;
430 }
431 
432 int mlx4_ib_find_real_gid(struct ib_device *ibdev, u8 port, __be64 guid)
433 {
434 	struct mlx4_ib_dev *dev = to_mdev(ibdev);
435 	int i;
436 
437 	for (i = 0; i < dev->dev->caps.sqp_demux; i++) {
438 		if (dev->sriov.demux[port - 1].guid_cache[i] == guid)
439 			return i;
440 	}
441 	return -1;
442 }
443 
444 
445 static int find_slave_port_pkey_ix(struct mlx4_ib_dev *dev, int slave,
446 				   u8 port, u16 pkey, u16 *ix)
447 {
448 	int i, ret;
449 	u8 unassigned_pkey_ix, pkey_ix, partial_ix = 0xFF;
450 	u16 slot_pkey;
451 
452 	if (slave == mlx4_master_func_num(dev->dev))
453 		return ib_find_cached_pkey(&dev->ib_dev, port, pkey, ix);
454 
455 	unassigned_pkey_ix = dev->dev->phys_caps.pkey_phys_table_len[port] - 1;
456 
457 	for (i = 0; i < dev->dev->caps.pkey_table_len[port]; i++) {
458 		if (dev->pkeys.virt2phys_pkey[slave][port - 1][i] == unassigned_pkey_ix)
459 			continue;
460 
461 		pkey_ix = dev->pkeys.virt2phys_pkey[slave][port - 1][i];
462 
463 		ret = ib_get_cached_pkey(&dev->ib_dev, port, pkey_ix, &slot_pkey);
464 		if (ret)
465 			continue;
466 		if ((slot_pkey & 0x7FFF) == (pkey & 0x7FFF)) {
467 			if (slot_pkey & 0x8000) {
468 				*ix = (u16) pkey_ix;
469 				return 0;
470 			} else {
471 				/* take first partial pkey index found */
472 				if (partial_ix == 0xFF)
473 					partial_ix = pkey_ix;
474 			}
475 		}
476 	}
477 
478 	if (partial_ix < 0xFF) {
479 		*ix = (u16) partial_ix;
480 		return 0;
481 	}
482 
483 	return -EINVAL;
484 }
485 
486 static int get_gids_from_l3_hdr(struct ib_grh *grh, union ib_gid *sgid,
487 				union ib_gid *dgid)
488 {
489 	int version = ib_get_rdma_header_version((const union rdma_network_hdr *)grh);
490 	enum rdma_network_type net_type;
491 
492 	if (version == 4)
493 		net_type = RDMA_NETWORK_IPV4;
494 	else if (version == 6)
495 		net_type = RDMA_NETWORK_IPV6;
496 	else
497 		return -EINVAL;
498 
499 	return ib_get_gids_from_rdma_hdr((union rdma_network_hdr *)grh, net_type,
500 					 sgid, dgid);
501 }
502 
503 int mlx4_ib_send_to_slave(struct mlx4_ib_dev *dev, int slave, u8 port,
504 			  enum ib_qp_type dest_qpt, struct ib_wc *wc,
505 			  struct ib_grh *grh, struct ib_mad *mad)
506 {
507 	struct ib_sge list;
508 	struct ib_ud_wr wr;
509 	const struct ib_send_wr *bad_wr;
510 	struct mlx4_ib_demux_pv_ctx *tun_ctx;
511 	struct mlx4_ib_demux_pv_qp *tun_qp;
512 	struct mlx4_rcv_tunnel_mad *tun_mad;
513 	struct rdma_ah_attr attr;
514 	struct ib_ah *ah;
515 	struct ib_qp *src_qp = NULL;
516 	unsigned tun_tx_ix = 0;
517 	int dqpn;
518 	int ret = 0;
519 	u16 tun_pkey_ix;
520 	u16 cached_pkey;
521 	u8 is_eth = dev->dev->caps.port_type[port] == MLX4_PORT_TYPE_ETH;
522 
523 	if (dest_qpt > IB_QPT_GSI)
524 		return -EINVAL;
525 
526 	tun_ctx = dev->sriov.demux[port-1].tun[slave];
527 
528 	/* check if proxy qp created */
529 	if (!tun_ctx || tun_ctx->state != DEMUX_PV_STATE_ACTIVE)
530 		return -EAGAIN;
531 
532 	if (!dest_qpt)
533 		tun_qp = &tun_ctx->qp[0];
534 	else
535 		tun_qp = &tun_ctx->qp[1];
536 
537 	/* compute P_Key index to put in tunnel header for slave */
538 	if (dest_qpt) {
539 		u16 pkey_ix;
540 		ret = ib_get_cached_pkey(&dev->ib_dev, port, wc->pkey_index, &cached_pkey);
541 		if (ret)
542 			return -EINVAL;
543 
544 		ret = find_slave_port_pkey_ix(dev, slave, port, cached_pkey, &pkey_ix);
545 		if (ret)
546 			return -EINVAL;
547 		tun_pkey_ix = pkey_ix;
548 	} else
549 		tun_pkey_ix = dev->pkeys.virt2phys_pkey[slave][port - 1][0];
550 
551 	dqpn = dev->dev->phys_caps.base_proxy_sqpn + 8 * slave + port + (dest_qpt * 2) - 1;
552 
553 	/* get tunnel tx data buf for slave */
554 	src_qp = tun_qp->qp;
555 
556 	/* create ah. Just need an empty one with the port num for the post send.
557 	 * The driver will set the force loopback bit in post_send */
558 	memset(&attr, 0, sizeof attr);
559 	attr.type = rdma_ah_find_type(&dev->ib_dev, port);
560 
561 	rdma_ah_set_port_num(&attr, port);
562 	if (is_eth) {
563 		union ib_gid sgid;
564 		union ib_gid dgid;
565 
566 		if (get_gids_from_l3_hdr(grh, &sgid, &dgid))
567 			return -EINVAL;
568 		rdma_ah_set_grh(&attr, &dgid, 0, 0, 0, 0);
569 	}
570 	ah = rdma_create_ah(tun_ctx->pd, &attr, 0);
571 	if (IS_ERR(ah))
572 		return -ENOMEM;
573 
574 	/* allocate tunnel tx buf after pass failure returns */
575 	spin_lock(&tun_qp->tx_lock);
576 	if (tun_qp->tx_ix_head - tun_qp->tx_ix_tail >=
577 	    (MLX4_NUM_TUNNEL_BUFS - 1))
578 		ret = -EAGAIN;
579 	else
580 		tun_tx_ix = (++tun_qp->tx_ix_head) & (MLX4_NUM_TUNNEL_BUFS - 1);
581 	spin_unlock(&tun_qp->tx_lock);
582 	if (ret)
583 		goto end;
584 
585 	tun_mad = (struct mlx4_rcv_tunnel_mad *) (tun_qp->tx_ring[tun_tx_ix].buf.addr);
586 	if (tun_qp->tx_ring[tun_tx_ix].ah)
587 		rdma_destroy_ah(tun_qp->tx_ring[tun_tx_ix].ah, 0);
588 	tun_qp->tx_ring[tun_tx_ix].ah = ah;
589 	ib_dma_sync_single_for_cpu(&dev->ib_dev,
590 				   tun_qp->tx_ring[tun_tx_ix].buf.map,
591 				   sizeof (struct mlx4_rcv_tunnel_mad),
592 				   DMA_TO_DEVICE);
593 
594 	/* copy over to tunnel buffer */
595 	if (grh)
596 		memcpy(&tun_mad->grh, grh, sizeof *grh);
597 	memcpy(&tun_mad->mad, mad, sizeof *mad);
598 
599 	/* adjust tunnel data */
600 	tun_mad->hdr.pkey_index = cpu_to_be16(tun_pkey_ix);
601 	tun_mad->hdr.flags_src_qp = cpu_to_be32(wc->src_qp & 0xFFFFFF);
602 	tun_mad->hdr.g_ml_path = (grh && (wc->wc_flags & IB_WC_GRH)) ? 0x80 : 0;
603 
604 	if (is_eth) {
605 		u16 vlan = 0;
606 		if (mlx4_get_slave_default_vlan(dev->dev, port, slave, &vlan,
607 						NULL)) {
608 			/* VST mode */
609 			if (vlan != wc->vlan_id)
610 				/* Packet vlan is not the VST-assigned vlan.
611 				 * Drop the packet.
612 				 */
613 				goto out;
614 			 else
615 				/* Remove the vlan tag before forwarding
616 				 * the packet to the VF.
617 				 */
618 				vlan = 0xffff;
619 		} else {
620 			vlan = wc->vlan_id;
621 		}
622 
623 		tun_mad->hdr.sl_vid = cpu_to_be16(vlan);
624 		memcpy((char *)&tun_mad->hdr.mac_31_0, &(wc->smac[0]), 4);
625 		memcpy((char *)&tun_mad->hdr.slid_mac_47_32, &(wc->smac[4]), 2);
626 	} else {
627 		tun_mad->hdr.sl_vid = cpu_to_be16(((u16)(wc->sl)) << 12);
628 		tun_mad->hdr.slid_mac_47_32 = ib_lid_be16(wc->slid);
629 	}
630 
631 	ib_dma_sync_single_for_device(&dev->ib_dev,
632 				      tun_qp->tx_ring[tun_tx_ix].buf.map,
633 				      sizeof (struct mlx4_rcv_tunnel_mad),
634 				      DMA_TO_DEVICE);
635 
636 	list.addr = tun_qp->tx_ring[tun_tx_ix].buf.map;
637 	list.length = sizeof (struct mlx4_rcv_tunnel_mad);
638 	list.lkey = tun_ctx->pd->local_dma_lkey;
639 
640 	wr.ah = ah;
641 	wr.port_num = port;
642 	wr.remote_qkey = IB_QP_SET_QKEY;
643 	wr.remote_qpn = dqpn;
644 	wr.wr.next = NULL;
645 	wr.wr.wr_id = ((u64) tun_tx_ix) | MLX4_TUN_SET_WRID_QPN(dest_qpt);
646 	wr.wr.sg_list = &list;
647 	wr.wr.num_sge = 1;
648 	wr.wr.opcode = IB_WR_SEND;
649 	wr.wr.send_flags = IB_SEND_SIGNALED;
650 
651 	ret = ib_post_send(src_qp, &wr.wr, &bad_wr);
652 	if (!ret)
653 		return 0;
654  out:
655 	spin_lock(&tun_qp->tx_lock);
656 	tun_qp->tx_ix_tail++;
657 	spin_unlock(&tun_qp->tx_lock);
658 	tun_qp->tx_ring[tun_tx_ix].ah = NULL;
659 end:
660 	rdma_destroy_ah(ah, 0);
661 	return ret;
662 }
663 
664 static int mlx4_ib_demux_mad(struct ib_device *ibdev, u8 port,
665 			struct ib_wc *wc, struct ib_grh *grh,
666 			struct ib_mad *mad)
667 {
668 	struct mlx4_ib_dev *dev = to_mdev(ibdev);
669 	int err, other_port;
670 	int slave = -1;
671 	u8 *slave_id;
672 	int is_eth = 0;
673 
674 	if (rdma_port_get_link_layer(ibdev, port) == IB_LINK_LAYER_INFINIBAND)
675 		is_eth = 0;
676 	else
677 		is_eth = 1;
678 
679 	if (is_eth) {
680 		union ib_gid dgid;
681 		union ib_gid sgid;
682 
683 		if (get_gids_from_l3_hdr(grh, &sgid, &dgid))
684 			return -EINVAL;
685 		if (!(wc->wc_flags & IB_WC_GRH)) {
686 			mlx4_ib_warn(ibdev, "RoCE grh not present.\n");
687 			return -EINVAL;
688 		}
689 		if (mad->mad_hdr.mgmt_class != IB_MGMT_CLASS_CM) {
690 			mlx4_ib_warn(ibdev, "RoCE mgmt class is not CM\n");
691 			return -EINVAL;
692 		}
693 		err = mlx4_get_slave_from_roce_gid(dev->dev, port, dgid.raw, &slave);
694 		if (err && mlx4_is_mf_bonded(dev->dev)) {
695 			other_port = (port == 1) ? 2 : 1;
696 			err = mlx4_get_slave_from_roce_gid(dev->dev, other_port, dgid.raw, &slave);
697 			if (!err) {
698 				port = other_port;
699 				pr_debug("resolved slave %d from gid %pI6 wire port %d other %d\n",
700 					 slave, grh->dgid.raw, port, other_port);
701 			}
702 		}
703 		if (err) {
704 			mlx4_ib_warn(ibdev, "failed matching grh\n");
705 			return -ENOENT;
706 		}
707 		if (slave >= dev->dev->caps.sqp_demux) {
708 			mlx4_ib_warn(ibdev, "slave id: %d is bigger than allowed:%d\n",
709 				     slave, dev->dev->caps.sqp_demux);
710 			return -ENOENT;
711 		}
712 
713 		if (mlx4_ib_demux_cm_handler(ibdev, port, NULL, mad))
714 			return 0;
715 
716 		err = mlx4_ib_send_to_slave(dev, slave, port, wc->qp->qp_type, wc, grh, mad);
717 		if (err)
718 			pr_debug("failed sending to slave %d via tunnel qp (%d)\n",
719 				 slave, err);
720 		return 0;
721 	}
722 
723 	/* Initially assume that this mad is for us */
724 	slave = mlx4_master_func_num(dev->dev);
725 
726 	/* See if the slave id is encoded in a response mad */
727 	if (mad->mad_hdr.method & 0x80) {
728 		slave_id = (u8 *) &mad->mad_hdr.tid;
729 		slave = *slave_id;
730 		if (slave != 255) /*255 indicates the dom0*/
731 			*slave_id = 0; /* remap tid */
732 	}
733 
734 	/* If a grh is present, we demux according to it */
735 	if (wc->wc_flags & IB_WC_GRH) {
736 		if (grh->dgid.global.interface_id ==
737 			cpu_to_be64(IB_SA_WELL_KNOWN_GUID) &&
738 		    grh->dgid.global.subnet_prefix == cpu_to_be64(
739 			atomic64_read(&dev->sriov.demux[port - 1].subnet_prefix))) {
740 			slave = 0;
741 		} else {
742 			slave = mlx4_ib_find_real_gid(ibdev, port,
743 						      grh->dgid.global.interface_id);
744 			if (slave < 0) {
745 				mlx4_ib_warn(ibdev, "failed matching grh\n");
746 				return -ENOENT;
747 			}
748 		}
749 	}
750 	/* Class-specific handling */
751 	switch (mad->mad_hdr.mgmt_class) {
752 	case IB_MGMT_CLASS_SUBN_LID_ROUTED:
753 	case IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE:
754 		/* 255 indicates the dom0 */
755 		if (slave != 255 && slave != mlx4_master_func_num(dev->dev)) {
756 			if (!mlx4_vf_smi_enabled(dev->dev, slave, port))
757 				return -EPERM;
758 			/* for a VF. drop unsolicited MADs */
759 			if (!(mad->mad_hdr.method & IB_MGMT_METHOD_RESP)) {
760 				mlx4_ib_warn(ibdev, "demux QP0. rejecting unsolicited mad for slave %d class 0x%x, method 0x%x\n",
761 					     slave, mad->mad_hdr.mgmt_class,
762 					     mad->mad_hdr.method);
763 				return -EINVAL;
764 			}
765 		}
766 		break;
767 	case IB_MGMT_CLASS_SUBN_ADM:
768 		if (mlx4_ib_demux_sa_handler(ibdev, port, slave,
769 					     (struct ib_sa_mad *) mad))
770 			return 0;
771 		break;
772 	case IB_MGMT_CLASS_CM:
773 		if (mlx4_ib_demux_cm_handler(ibdev, port, &slave, mad))
774 			return 0;
775 		break;
776 	case IB_MGMT_CLASS_DEVICE_MGMT:
777 		if (mad->mad_hdr.method != IB_MGMT_METHOD_GET_RESP)
778 			return 0;
779 		break;
780 	default:
781 		/* Drop unsupported classes for slaves in tunnel mode */
782 		if (slave != mlx4_master_func_num(dev->dev)) {
783 			pr_debug("dropping unsupported ingress mad from class:%d "
784 				 "for slave:%d\n", mad->mad_hdr.mgmt_class, slave);
785 			return 0;
786 		}
787 	}
788 	/*make sure that no slave==255 was not handled yet.*/
789 	if (slave >= dev->dev->caps.sqp_demux) {
790 		mlx4_ib_warn(ibdev, "slave id: %d is bigger than allowed:%d\n",
791 			     slave, dev->dev->caps.sqp_demux);
792 		return -ENOENT;
793 	}
794 
795 	err = mlx4_ib_send_to_slave(dev, slave, port, wc->qp->qp_type, wc, grh, mad);
796 	if (err)
797 		pr_debug("failed sending to slave %d via tunnel qp (%d)\n",
798 			 slave, err);
799 	return 0;
800 }
801 
802 static int ib_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
803 			const struct ib_wc *in_wc, const struct ib_grh *in_grh,
804 			const struct ib_mad *in_mad, struct ib_mad *out_mad)
805 {
806 	u16 slid, prev_lid = 0;
807 	int err;
808 	struct ib_port_attr pattr;
809 
810 	if (in_wc && in_wc->qp) {
811 		pr_debug("received MAD: port:%d slid:%d sqpn:%d "
812 			 "dlid_bits:%d dqpn:%d wc_flags:0x%x tid:%016llx cls:%x mtd:%x atr:%x\n",
813 			 port_num,
814 			 in_wc->slid, in_wc->src_qp,
815 			 in_wc->dlid_path_bits,
816 			 in_wc->qp->qp_num,
817 			 in_wc->wc_flags,
818 			 be64_to_cpu(in_mad->mad_hdr.tid),
819 			 in_mad->mad_hdr.mgmt_class, in_mad->mad_hdr.method,
820 			 be16_to_cpu(in_mad->mad_hdr.attr_id));
821 		if (in_wc->wc_flags & IB_WC_GRH) {
822 			pr_debug("sgid_hi:0x%016llx sgid_lo:0x%016llx\n",
823 				 be64_to_cpu(in_grh->sgid.global.subnet_prefix),
824 				 be64_to_cpu(in_grh->sgid.global.interface_id));
825 			pr_debug("dgid_hi:0x%016llx dgid_lo:0x%016llx\n",
826 				 be64_to_cpu(in_grh->dgid.global.subnet_prefix),
827 				 be64_to_cpu(in_grh->dgid.global.interface_id));
828 		}
829 	}
830 
831 	slid = in_wc ? ib_lid_cpu16(in_wc->slid) : be16_to_cpu(IB_LID_PERMISSIVE);
832 
833 	if (in_mad->mad_hdr.method == IB_MGMT_METHOD_TRAP && slid == 0) {
834 		forward_trap(to_mdev(ibdev), port_num, in_mad);
835 		return IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_CONSUMED;
836 	}
837 
838 	if (in_mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_SUBN_LID_ROUTED ||
839 	    in_mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE) {
840 		if (in_mad->mad_hdr.method   != IB_MGMT_METHOD_GET &&
841 		    in_mad->mad_hdr.method   != IB_MGMT_METHOD_SET &&
842 		    in_mad->mad_hdr.method   != IB_MGMT_METHOD_TRAP_REPRESS)
843 			return IB_MAD_RESULT_SUCCESS;
844 
845 		/*
846 		 * Don't process SMInfo queries -- the SMA can't handle them.
847 		 */
848 		if (in_mad->mad_hdr.attr_id == IB_SMP_ATTR_SM_INFO)
849 			return IB_MAD_RESULT_SUCCESS;
850 	} else if (in_mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_PERF_MGMT ||
851 		   in_mad->mad_hdr.mgmt_class == MLX4_IB_VENDOR_CLASS1   ||
852 		   in_mad->mad_hdr.mgmt_class == MLX4_IB_VENDOR_CLASS2   ||
853 		   in_mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_CONG_MGMT) {
854 		if (in_mad->mad_hdr.method  != IB_MGMT_METHOD_GET &&
855 		    in_mad->mad_hdr.method  != IB_MGMT_METHOD_SET)
856 			return IB_MAD_RESULT_SUCCESS;
857 	} else
858 		return IB_MAD_RESULT_SUCCESS;
859 
860 	if ((in_mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_SUBN_LID_ROUTED ||
861 	     in_mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE) &&
862 	    in_mad->mad_hdr.method == IB_MGMT_METHOD_SET &&
863 	    in_mad->mad_hdr.attr_id == IB_SMP_ATTR_PORT_INFO &&
864 	    !ib_query_port(ibdev, port_num, &pattr))
865 		prev_lid = ib_lid_cpu16(pattr.lid);
866 
867 	err = mlx4_MAD_IFC(to_mdev(ibdev),
868 			   (mad_flags & IB_MAD_IGNORE_MKEY ? MLX4_MAD_IFC_IGNORE_MKEY : 0) |
869 			   (mad_flags & IB_MAD_IGNORE_BKEY ? MLX4_MAD_IFC_IGNORE_BKEY : 0) |
870 			   MLX4_MAD_IFC_NET_VIEW,
871 			   port_num, in_wc, in_grh, in_mad, out_mad);
872 	if (err)
873 		return IB_MAD_RESULT_FAILURE;
874 
875 	if (!out_mad->mad_hdr.status) {
876 		smp_snoop(ibdev, port_num, in_mad, prev_lid);
877 		/* slaves get node desc from FW */
878 		if (!mlx4_is_slave(to_mdev(ibdev)->dev))
879 			node_desc_override(ibdev, out_mad);
880 	}
881 
882 	/* set return bit in status of directed route responses */
883 	if (in_mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE)
884 		out_mad->mad_hdr.status |= cpu_to_be16(1 << 15);
885 
886 	if (in_mad->mad_hdr.method == IB_MGMT_METHOD_TRAP_REPRESS)
887 		/* no response for trap repress */
888 		return IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_CONSUMED;
889 
890 	return IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_REPLY;
891 }
892 
893 static void edit_counter(struct mlx4_counter *cnt, void *counters,
894 			 __be16 attr_id)
895 {
896 	switch (attr_id) {
897 	case IB_PMA_PORT_COUNTERS:
898 	{
899 		struct ib_pma_portcounters *pma_cnt =
900 			(struct ib_pma_portcounters *)counters;
901 
902 		ASSIGN_32BIT_COUNTER(pma_cnt->port_xmit_data,
903 				     (be64_to_cpu(cnt->tx_bytes) >> 2));
904 		ASSIGN_32BIT_COUNTER(pma_cnt->port_rcv_data,
905 				     (be64_to_cpu(cnt->rx_bytes) >> 2));
906 		ASSIGN_32BIT_COUNTER(pma_cnt->port_xmit_packets,
907 				     be64_to_cpu(cnt->tx_frames));
908 		ASSIGN_32BIT_COUNTER(pma_cnt->port_rcv_packets,
909 				     be64_to_cpu(cnt->rx_frames));
910 		break;
911 	}
912 	case IB_PMA_PORT_COUNTERS_EXT:
913 	{
914 		struct ib_pma_portcounters_ext *pma_cnt_ext =
915 			(struct ib_pma_portcounters_ext *)counters;
916 
917 		pma_cnt_ext->port_xmit_data =
918 			cpu_to_be64(be64_to_cpu(cnt->tx_bytes) >> 2);
919 		pma_cnt_ext->port_rcv_data =
920 			cpu_to_be64(be64_to_cpu(cnt->rx_bytes) >> 2);
921 		pma_cnt_ext->port_xmit_packets = cnt->tx_frames;
922 		pma_cnt_ext->port_rcv_packets = cnt->rx_frames;
923 		break;
924 	}
925 	}
926 }
927 
928 static int iboe_process_mad_port_info(void *out_mad)
929 {
930 	struct ib_class_port_info cpi = {};
931 
932 	cpi.capability_mask = IB_PMA_CLASS_CAP_EXT_WIDTH;
933 	memcpy(out_mad, &cpi, sizeof(cpi));
934 	return IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_REPLY;
935 }
936 
937 static int iboe_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
938 			const struct ib_wc *in_wc, const struct ib_grh *in_grh,
939 			const struct ib_mad *in_mad, struct ib_mad *out_mad)
940 {
941 	struct mlx4_counter counter_stats;
942 	struct mlx4_ib_dev *dev = to_mdev(ibdev);
943 	struct counter_index *tmp_counter;
944 	int err = IB_MAD_RESULT_FAILURE, stats_avail = 0;
945 
946 	if (in_mad->mad_hdr.mgmt_class != IB_MGMT_CLASS_PERF_MGMT)
947 		return -EINVAL;
948 
949 	if (in_mad->mad_hdr.attr_id == IB_PMA_CLASS_PORT_INFO)
950 		return iboe_process_mad_port_info((void *)(out_mad->data + 40));
951 
952 	memset(&counter_stats, 0, sizeof(counter_stats));
953 	mutex_lock(&dev->counters_table[port_num - 1].mutex);
954 	list_for_each_entry(tmp_counter,
955 			    &dev->counters_table[port_num - 1].counters_list,
956 			    list) {
957 		err = mlx4_get_counter_stats(dev->dev,
958 					     tmp_counter->index,
959 					     &counter_stats, 0);
960 		if (err) {
961 			err = IB_MAD_RESULT_FAILURE;
962 			stats_avail = 0;
963 			break;
964 		}
965 		stats_avail = 1;
966 	}
967 	mutex_unlock(&dev->counters_table[port_num - 1].mutex);
968 	if (stats_avail) {
969 		switch (counter_stats.counter_mode & 0xf) {
970 		case 0:
971 			edit_counter(&counter_stats,
972 				     (void *)(out_mad->data + 40),
973 				     in_mad->mad_hdr.attr_id);
974 			err = IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_REPLY;
975 			break;
976 		default:
977 			err = IB_MAD_RESULT_FAILURE;
978 		}
979 	}
980 
981 	return err;
982 }
983 
984 int mlx4_ib_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
985 			const struct ib_wc *in_wc, const struct ib_grh *in_grh,
986 			const struct ib_mad *in, struct ib_mad *out,
987 			size_t *out_mad_size, u16 *out_mad_pkey_index)
988 {
989 	struct mlx4_ib_dev *dev = to_mdev(ibdev);
990 	enum rdma_link_layer link = rdma_port_get_link_layer(ibdev, port_num);
991 
992 	/* iboe_process_mad() which uses the HCA flow-counters to implement IB PMA
993 	 * queries, should be called only by VFs and for that specific purpose
994 	 */
995 	if (link == IB_LINK_LAYER_INFINIBAND) {
996 		if (mlx4_is_slave(dev->dev) &&
997 		    (in->mad_hdr.mgmt_class == IB_MGMT_CLASS_PERF_MGMT &&
998 		     (in->mad_hdr.attr_id == IB_PMA_PORT_COUNTERS ||
999 		      in->mad_hdr.attr_id == IB_PMA_PORT_COUNTERS_EXT ||
1000 		      in->mad_hdr.attr_id == IB_PMA_CLASS_PORT_INFO)))
1001 			return iboe_process_mad(ibdev, mad_flags, port_num,
1002 						in_wc, in_grh, in, out);
1003 
1004 		return ib_process_mad(ibdev, mad_flags, port_num, in_wc, in_grh,
1005 				      in, out);
1006 	}
1007 
1008 	if (link == IB_LINK_LAYER_ETHERNET)
1009 		return iboe_process_mad(ibdev, mad_flags, port_num, in_wc,
1010 					in_grh, in, out);
1011 
1012 	return -EINVAL;
1013 }
1014 
1015 static void send_handler(struct ib_mad_agent *agent,
1016 			 struct ib_mad_send_wc *mad_send_wc)
1017 {
1018 	if (mad_send_wc->send_buf->context[0])
1019 		rdma_destroy_ah(mad_send_wc->send_buf->context[0], 0);
1020 	ib_free_send_mad(mad_send_wc->send_buf);
1021 }
1022 
1023 int mlx4_ib_mad_init(struct mlx4_ib_dev *dev)
1024 {
1025 	struct ib_mad_agent *agent;
1026 	int p, q;
1027 	int ret;
1028 	enum rdma_link_layer ll;
1029 
1030 	for (p = 0; p < dev->num_ports; ++p) {
1031 		ll = rdma_port_get_link_layer(&dev->ib_dev, p + 1);
1032 		for (q = 0; q <= 1; ++q) {
1033 			if (ll == IB_LINK_LAYER_INFINIBAND) {
1034 				agent = ib_register_mad_agent(&dev->ib_dev, p + 1,
1035 							      q ? IB_QPT_GSI : IB_QPT_SMI,
1036 							      NULL, 0, send_handler,
1037 							      NULL, NULL, 0);
1038 				if (IS_ERR(agent)) {
1039 					ret = PTR_ERR(agent);
1040 					goto err;
1041 				}
1042 				dev->send_agent[p][q] = agent;
1043 			} else
1044 				dev->send_agent[p][q] = NULL;
1045 		}
1046 	}
1047 
1048 	return 0;
1049 
1050 err:
1051 	for (p = 0; p < dev->num_ports; ++p)
1052 		for (q = 0; q <= 1; ++q)
1053 			if (dev->send_agent[p][q])
1054 				ib_unregister_mad_agent(dev->send_agent[p][q]);
1055 
1056 	return ret;
1057 }
1058 
1059 void mlx4_ib_mad_cleanup(struct mlx4_ib_dev *dev)
1060 {
1061 	struct ib_mad_agent *agent;
1062 	int p, q;
1063 
1064 	for (p = 0; p < dev->num_ports; ++p) {
1065 		for (q = 0; q <= 1; ++q) {
1066 			agent = dev->send_agent[p][q];
1067 			if (agent) {
1068 				dev->send_agent[p][q] = NULL;
1069 				ib_unregister_mad_agent(agent);
1070 			}
1071 		}
1072 
1073 		if (dev->sm_ah[p])
1074 			rdma_destroy_ah(dev->sm_ah[p], 0);
1075 	}
1076 }
1077 
1078 static void handle_lid_change_event(struct mlx4_ib_dev *dev, u8 port_num)
1079 {
1080 	mlx4_ib_dispatch_event(dev, port_num, IB_EVENT_LID_CHANGE);
1081 
1082 	if (mlx4_is_master(dev->dev) && !dev->sriov.is_going_down)
1083 		mlx4_gen_slaves_port_mgt_ev(dev->dev, port_num,
1084 					    MLX4_EQ_PORT_INFO_LID_CHANGE_MASK);
1085 }
1086 
1087 static void handle_client_rereg_event(struct mlx4_ib_dev *dev, u8 port_num)
1088 {
1089 	/* re-configure the alias-guid and mcg's */
1090 	if (mlx4_is_master(dev->dev)) {
1091 		mlx4_ib_invalidate_all_guid_record(dev, port_num);
1092 
1093 		if (!dev->sriov.is_going_down) {
1094 			mlx4_ib_mcg_port_cleanup(&dev->sriov.demux[port_num - 1], 0);
1095 			mlx4_gen_slaves_port_mgt_ev(dev->dev, port_num,
1096 						    MLX4_EQ_PORT_INFO_CLIENT_REREG_MASK);
1097 		}
1098 	}
1099 
1100 	/* Update the sl to vl table from inside client rereg
1101 	 * only if in secure-host mode (snooping is not possible)
1102 	 * and the sl-to-vl change event is not generated by FW.
1103 	 */
1104 	if (!mlx4_is_slave(dev->dev) &&
1105 	    dev->dev->flags & MLX4_FLAG_SECURE_HOST &&
1106 	    !(dev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_SL_TO_VL_CHANGE_EVENT)) {
1107 		if (mlx4_is_master(dev->dev))
1108 			/* already in work queue from mlx4_ib_event queueing
1109 			 * mlx4_handle_port_mgmt_change_event, which calls
1110 			 * this procedure. Therefore, call sl2vl_update directly.
1111 			 */
1112 			mlx4_ib_sl2vl_update(dev, port_num);
1113 		else
1114 			mlx4_sched_ib_sl2vl_update_work(dev, port_num);
1115 	}
1116 	mlx4_ib_dispatch_event(dev, port_num, IB_EVENT_CLIENT_REREGISTER);
1117 }
1118 
1119 static void propagate_pkey_ev(struct mlx4_ib_dev *dev, int port_num,
1120 			      struct mlx4_eqe *eqe)
1121 {
1122 	__propagate_pkey_ev(dev, port_num, GET_BLK_PTR_FROM_EQE(eqe),
1123 			    GET_MASK_FROM_EQE(eqe));
1124 }
1125 
1126 static void handle_slaves_guid_change(struct mlx4_ib_dev *dev, u8 port_num,
1127 				      u32 guid_tbl_blk_num, u32 change_bitmap)
1128 {
1129 	struct ib_smp *in_mad  = NULL;
1130 	struct ib_smp *out_mad  = NULL;
1131 	u16 i;
1132 
1133 	if (!mlx4_is_mfunc(dev->dev) || !mlx4_is_master(dev->dev))
1134 		return;
1135 
1136 	in_mad  = kmalloc(sizeof *in_mad, GFP_KERNEL);
1137 	out_mad = kmalloc(sizeof *out_mad, GFP_KERNEL);
1138 	if (!in_mad || !out_mad)
1139 		goto out;
1140 
1141 	guid_tbl_blk_num  *= 4;
1142 
1143 	for (i = 0; i < 4; i++) {
1144 		if (change_bitmap && (!((change_bitmap >> (8 * i)) & 0xff)))
1145 			continue;
1146 		memset(in_mad, 0, sizeof *in_mad);
1147 		memset(out_mad, 0, sizeof *out_mad);
1148 
1149 		in_mad->base_version  = 1;
1150 		in_mad->mgmt_class    = IB_MGMT_CLASS_SUBN_LID_ROUTED;
1151 		in_mad->class_version = 1;
1152 		in_mad->method        = IB_MGMT_METHOD_GET;
1153 		in_mad->attr_id       = IB_SMP_ATTR_GUID_INFO;
1154 		in_mad->attr_mod      = cpu_to_be32(guid_tbl_blk_num + i);
1155 
1156 		if (mlx4_MAD_IFC(dev,
1157 				 MLX4_MAD_IFC_IGNORE_KEYS | MLX4_MAD_IFC_NET_VIEW,
1158 				 port_num, NULL, NULL, in_mad, out_mad)) {
1159 			mlx4_ib_warn(&dev->ib_dev, "Failed in get GUID INFO MAD_IFC\n");
1160 			goto out;
1161 		}
1162 
1163 		mlx4_ib_update_cache_on_guid_change(dev, guid_tbl_blk_num + i,
1164 						    port_num,
1165 						    (u8 *)(&((struct ib_smp *)out_mad)->data));
1166 		mlx4_ib_notify_slaves_on_guid_change(dev, guid_tbl_blk_num + i,
1167 						     port_num,
1168 						     (u8 *)(&((struct ib_smp *)out_mad)->data));
1169 	}
1170 
1171 out:
1172 	kfree(in_mad);
1173 	kfree(out_mad);
1174 	return;
1175 }
1176 
1177 void handle_port_mgmt_change_event(struct work_struct *work)
1178 {
1179 	struct ib_event_work *ew = container_of(work, struct ib_event_work, work);
1180 	struct mlx4_ib_dev *dev = ew->ib_dev;
1181 	struct mlx4_eqe *eqe = &(ew->ib_eqe);
1182 	u8 port = eqe->event.port_mgmt_change.port;
1183 	u32 changed_attr;
1184 	u32 tbl_block;
1185 	u32 change_bitmap;
1186 
1187 	switch (eqe->subtype) {
1188 	case MLX4_DEV_PMC_SUBTYPE_PORT_INFO:
1189 		changed_attr = be32_to_cpu(eqe->event.port_mgmt_change.params.port_info.changed_attr);
1190 
1191 		/* Update the SM ah - This should be done before handling
1192 		   the other changed attributes so that MADs can be sent to the SM */
1193 		if (changed_attr & MSTR_SM_CHANGE_MASK) {
1194 			u16 lid = be16_to_cpu(eqe->event.port_mgmt_change.params.port_info.mstr_sm_lid);
1195 			u8 sl = eqe->event.port_mgmt_change.params.port_info.mstr_sm_sl & 0xf;
1196 			update_sm_ah(dev, port, lid, sl);
1197 		}
1198 
1199 		/* Check if it is a lid change event */
1200 		if (changed_attr & MLX4_EQ_PORT_INFO_LID_CHANGE_MASK)
1201 			handle_lid_change_event(dev, port);
1202 
1203 		/* Generate GUID changed event */
1204 		if (changed_attr & MLX4_EQ_PORT_INFO_GID_PFX_CHANGE_MASK) {
1205 			if (mlx4_is_master(dev->dev)) {
1206 				union ib_gid gid;
1207 				int err = 0;
1208 
1209 				if (!eqe->event.port_mgmt_change.params.port_info.gid_prefix)
1210 					err = __mlx4_ib_query_gid(&dev->ib_dev, port, 0, &gid, 1);
1211 				else
1212 					gid.global.subnet_prefix =
1213 						eqe->event.port_mgmt_change.params.port_info.gid_prefix;
1214 				if (err) {
1215 					pr_warn("Could not change QP1 subnet prefix for port %d: query_gid error (%d)\n",
1216 						port, err);
1217 				} else {
1218 					pr_debug("Changing QP1 subnet prefix for port %d. old=0x%llx. new=0x%llx\n",
1219 						 port,
1220 						 (u64)atomic64_read(&dev->sriov.demux[port - 1].subnet_prefix),
1221 						 be64_to_cpu(gid.global.subnet_prefix));
1222 					atomic64_set(&dev->sriov.demux[port - 1].subnet_prefix,
1223 						     be64_to_cpu(gid.global.subnet_prefix));
1224 				}
1225 			}
1226 			mlx4_ib_dispatch_event(dev, port, IB_EVENT_GID_CHANGE);
1227 			/*if master, notify all slaves*/
1228 			if (mlx4_is_master(dev->dev))
1229 				mlx4_gen_slaves_port_mgt_ev(dev->dev, port,
1230 							    MLX4_EQ_PORT_INFO_GID_PFX_CHANGE_MASK);
1231 		}
1232 
1233 		if (changed_attr & MLX4_EQ_PORT_INFO_CLIENT_REREG_MASK)
1234 			handle_client_rereg_event(dev, port);
1235 		break;
1236 
1237 	case MLX4_DEV_PMC_SUBTYPE_PKEY_TABLE:
1238 		mlx4_ib_dispatch_event(dev, port, IB_EVENT_PKEY_CHANGE);
1239 		if (mlx4_is_master(dev->dev) && !dev->sriov.is_going_down)
1240 			propagate_pkey_ev(dev, port, eqe);
1241 		break;
1242 	case MLX4_DEV_PMC_SUBTYPE_GUID_INFO:
1243 		/* paravirtualized master's guid is guid 0 -- does not change */
1244 		if (!mlx4_is_master(dev->dev))
1245 			mlx4_ib_dispatch_event(dev, port, IB_EVENT_GID_CHANGE);
1246 		/*if master, notify relevant slaves*/
1247 		else if (!dev->sriov.is_going_down) {
1248 			tbl_block = GET_BLK_PTR_FROM_EQE(eqe);
1249 			change_bitmap = GET_MASK_FROM_EQE(eqe);
1250 			handle_slaves_guid_change(dev, port, tbl_block, change_bitmap);
1251 		}
1252 		break;
1253 
1254 	case MLX4_DEV_PMC_SUBTYPE_SL_TO_VL_MAP:
1255 		/* cache sl to vl mapping changes for use in
1256 		 * filling QP1 LRH VL field when sending packets
1257 		 */
1258 		if (!mlx4_is_slave(dev->dev)) {
1259 			union sl2vl_tbl_to_u64 sl2vl64;
1260 			int jj;
1261 
1262 			for (jj = 0; jj < 8; jj++) {
1263 				sl2vl64.sl8[jj] =
1264 					eqe->event.port_mgmt_change.params.sl2vl_tbl_change_info.sl2vl_table[jj];
1265 				pr_debug("port %u, sl2vl[%d] = %02x\n",
1266 					 port, jj, sl2vl64.sl8[jj]);
1267 			}
1268 			atomic64_set(&dev->sl2vl[port - 1], sl2vl64.sl64);
1269 		}
1270 		break;
1271 	default:
1272 		pr_warn("Unsupported subtype 0x%x for "
1273 			"Port Management Change event\n", eqe->subtype);
1274 	}
1275 
1276 	kfree(ew);
1277 }
1278 
1279 void mlx4_ib_dispatch_event(struct mlx4_ib_dev *dev, u8 port_num,
1280 			    enum ib_event_type type)
1281 {
1282 	struct ib_event event;
1283 
1284 	event.device		= &dev->ib_dev;
1285 	event.element.port_num	= port_num;
1286 	event.event		= type;
1287 
1288 	ib_dispatch_event(&event);
1289 }
1290 
1291 static void mlx4_ib_tunnel_comp_handler(struct ib_cq *cq, void *arg)
1292 {
1293 	unsigned long flags;
1294 	struct mlx4_ib_demux_pv_ctx *ctx = cq->cq_context;
1295 	struct mlx4_ib_dev *dev = to_mdev(ctx->ib_dev);
1296 	spin_lock_irqsave(&dev->sriov.going_down_lock, flags);
1297 	if (!dev->sriov.is_going_down && ctx->state == DEMUX_PV_STATE_ACTIVE)
1298 		queue_work(ctx->wq, &ctx->work);
1299 	spin_unlock_irqrestore(&dev->sriov.going_down_lock, flags);
1300 }
1301 
1302 static int mlx4_ib_post_pv_qp_buf(struct mlx4_ib_demux_pv_ctx *ctx,
1303 				  struct mlx4_ib_demux_pv_qp *tun_qp,
1304 				  int index)
1305 {
1306 	struct ib_sge sg_list;
1307 	struct ib_recv_wr recv_wr;
1308 	const struct ib_recv_wr *bad_recv_wr;
1309 	int size;
1310 
1311 	size = (tun_qp->qp->qp_type == IB_QPT_UD) ?
1312 		sizeof (struct mlx4_tunnel_mad) : sizeof (struct mlx4_mad_rcv_buf);
1313 
1314 	sg_list.addr = tun_qp->ring[index].map;
1315 	sg_list.length = size;
1316 	sg_list.lkey = ctx->pd->local_dma_lkey;
1317 
1318 	recv_wr.next = NULL;
1319 	recv_wr.sg_list = &sg_list;
1320 	recv_wr.num_sge = 1;
1321 	recv_wr.wr_id = (u64) index | MLX4_TUN_WRID_RECV |
1322 		MLX4_TUN_SET_WRID_QPN(tun_qp->proxy_qpt);
1323 	ib_dma_sync_single_for_device(ctx->ib_dev, tun_qp->ring[index].map,
1324 				      size, DMA_FROM_DEVICE);
1325 	return ib_post_recv(tun_qp->qp, &recv_wr, &bad_recv_wr);
1326 }
1327 
1328 static int mlx4_ib_multiplex_sa_handler(struct ib_device *ibdev, int port,
1329 		int slave, struct ib_sa_mad *sa_mad)
1330 {
1331 	int ret = 0;
1332 
1333 	/* dispatch to different sa handlers */
1334 	switch (be16_to_cpu(sa_mad->mad_hdr.attr_id)) {
1335 	case IB_SA_ATTR_MC_MEMBER_REC:
1336 		ret = mlx4_ib_mcg_multiplex_handler(ibdev, port, slave, sa_mad);
1337 		break;
1338 	default:
1339 		break;
1340 	}
1341 	return ret;
1342 }
1343 
1344 static int is_proxy_qp0(struct mlx4_ib_dev *dev, int qpn, int slave)
1345 {
1346 	int proxy_start = dev->dev->phys_caps.base_proxy_sqpn + 8 * slave;
1347 
1348 	return (qpn >= proxy_start && qpn <= proxy_start + 1);
1349 }
1350 
1351 
1352 int mlx4_ib_send_to_wire(struct mlx4_ib_dev *dev, int slave, u8 port,
1353 			 enum ib_qp_type dest_qpt, u16 pkey_index,
1354 			 u32 remote_qpn, u32 qkey, struct rdma_ah_attr *attr,
1355 			 u8 *s_mac, u16 vlan_id, struct ib_mad *mad)
1356 {
1357 	struct ib_sge list;
1358 	struct ib_ud_wr wr;
1359 	const struct ib_send_wr *bad_wr;
1360 	struct mlx4_ib_demux_pv_ctx *sqp_ctx;
1361 	struct mlx4_ib_demux_pv_qp *sqp;
1362 	struct mlx4_mad_snd_buf *sqp_mad;
1363 	struct ib_ah *ah;
1364 	struct ib_qp *send_qp = NULL;
1365 	unsigned wire_tx_ix = 0;
1366 	u16 wire_pkey_ix;
1367 	int src_qpnum;
1368 	int ret;
1369 
1370 	sqp_ctx = dev->sriov.sqps[port-1];
1371 
1372 	/* check if proxy qp created */
1373 	if (!sqp_ctx || sqp_ctx->state != DEMUX_PV_STATE_ACTIVE)
1374 		return -EAGAIN;
1375 
1376 	if (dest_qpt == IB_QPT_SMI) {
1377 		src_qpnum = 0;
1378 		sqp = &sqp_ctx->qp[0];
1379 		wire_pkey_ix = dev->pkeys.virt2phys_pkey[slave][port - 1][0];
1380 	} else {
1381 		src_qpnum = 1;
1382 		sqp = &sqp_ctx->qp[1];
1383 		wire_pkey_ix = dev->pkeys.virt2phys_pkey[slave][port - 1][pkey_index];
1384 	}
1385 
1386 	send_qp = sqp->qp;
1387 
1388 	ah = rdma_zalloc_drv_obj(sqp_ctx->pd->device, ib_ah);
1389 	if (!ah)
1390 		return -ENOMEM;
1391 
1392 	ah->device = sqp_ctx->pd->device;
1393 	ah->pd = sqp_ctx->pd;
1394 
1395 	/* create ah */
1396 	ret = mlx4_ib_create_ah_slave(ah, attr,
1397 				      rdma_ah_retrieve_grh(attr)->sgid_index,
1398 				      s_mac, vlan_id);
1399 	if (ret)
1400 		goto out;
1401 
1402 	spin_lock(&sqp->tx_lock);
1403 	if (sqp->tx_ix_head - sqp->tx_ix_tail >=
1404 	    (MLX4_NUM_TUNNEL_BUFS - 1))
1405 		ret = -EAGAIN;
1406 	else
1407 		wire_tx_ix = (++sqp->tx_ix_head) & (MLX4_NUM_TUNNEL_BUFS - 1);
1408 	spin_unlock(&sqp->tx_lock);
1409 	if (ret)
1410 		goto out;
1411 
1412 	sqp_mad = (struct mlx4_mad_snd_buf *) (sqp->tx_ring[wire_tx_ix].buf.addr);
1413 	kfree(sqp->tx_ring[wire_tx_ix].ah);
1414 	sqp->tx_ring[wire_tx_ix].ah = ah;
1415 	ib_dma_sync_single_for_cpu(&dev->ib_dev,
1416 				   sqp->tx_ring[wire_tx_ix].buf.map,
1417 				   sizeof (struct mlx4_mad_snd_buf),
1418 				   DMA_TO_DEVICE);
1419 
1420 	memcpy(&sqp_mad->payload, mad, sizeof *mad);
1421 
1422 	ib_dma_sync_single_for_device(&dev->ib_dev,
1423 				      sqp->tx_ring[wire_tx_ix].buf.map,
1424 				      sizeof (struct mlx4_mad_snd_buf),
1425 				      DMA_TO_DEVICE);
1426 
1427 	list.addr = sqp->tx_ring[wire_tx_ix].buf.map;
1428 	list.length = sizeof (struct mlx4_mad_snd_buf);
1429 	list.lkey = sqp_ctx->pd->local_dma_lkey;
1430 
1431 	wr.ah = ah;
1432 	wr.port_num = port;
1433 	wr.pkey_index = wire_pkey_ix;
1434 	wr.remote_qkey = qkey;
1435 	wr.remote_qpn = remote_qpn;
1436 	wr.wr.next = NULL;
1437 	wr.wr.wr_id = ((u64) wire_tx_ix) | MLX4_TUN_SET_WRID_QPN(src_qpnum);
1438 	wr.wr.sg_list = &list;
1439 	wr.wr.num_sge = 1;
1440 	wr.wr.opcode = IB_WR_SEND;
1441 	wr.wr.send_flags = IB_SEND_SIGNALED;
1442 
1443 	ret = ib_post_send(send_qp, &wr.wr, &bad_wr);
1444 	if (!ret)
1445 		return 0;
1446 
1447 	spin_lock(&sqp->tx_lock);
1448 	sqp->tx_ix_tail++;
1449 	spin_unlock(&sqp->tx_lock);
1450 	sqp->tx_ring[wire_tx_ix].ah = NULL;
1451 out:
1452 	kfree(ah);
1453 	return ret;
1454 }
1455 
1456 static int get_slave_base_gid_ix(struct mlx4_ib_dev *dev, int slave, int port)
1457 {
1458 	if (rdma_port_get_link_layer(&dev->ib_dev, port) == IB_LINK_LAYER_INFINIBAND)
1459 		return slave;
1460 	return mlx4_get_base_gid_ix(dev->dev, slave, port);
1461 }
1462 
1463 static void fill_in_real_sgid_index(struct mlx4_ib_dev *dev, int slave, int port,
1464 				    struct rdma_ah_attr *ah_attr)
1465 {
1466 	struct ib_global_route *grh = rdma_ah_retrieve_grh(ah_attr);
1467 	if (rdma_port_get_link_layer(&dev->ib_dev, port) == IB_LINK_LAYER_INFINIBAND)
1468 		grh->sgid_index = slave;
1469 	else
1470 		grh->sgid_index += get_slave_base_gid_ix(dev, slave, port);
1471 }
1472 
1473 static void mlx4_ib_multiplex_mad(struct mlx4_ib_demux_pv_ctx *ctx, struct ib_wc *wc)
1474 {
1475 	struct mlx4_ib_dev *dev = to_mdev(ctx->ib_dev);
1476 	struct mlx4_ib_demux_pv_qp *tun_qp = &ctx->qp[MLX4_TUN_WRID_QPN(wc->wr_id)];
1477 	int wr_ix = wc->wr_id & (MLX4_NUM_TUNNEL_BUFS - 1);
1478 	struct mlx4_tunnel_mad *tunnel = tun_qp->ring[wr_ix].addr;
1479 	struct mlx4_ib_ah ah;
1480 	struct rdma_ah_attr ah_attr;
1481 	u8 *slave_id;
1482 	int slave;
1483 	int port;
1484 	u16 vlan_id;
1485 	u8 qos;
1486 	u8 *dmac;
1487 
1488 	/* Get slave that sent this packet */
1489 	if (wc->src_qp < dev->dev->phys_caps.base_proxy_sqpn ||
1490 	    wc->src_qp >= dev->dev->phys_caps.base_proxy_sqpn + 8 * MLX4_MFUNC_MAX ||
1491 	    (wc->src_qp & 0x1) != ctx->port - 1 ||
1492 	    wc->src_qp & 0x4) {
1493 		mlx4_ib_warn(ctx->ib_dev, "can't multiplex bad sqp:%d\n", wc->src_qp);
1494 		return;
1495 	}
1496 	slave = ((wc->src_qp & ~0x7) - dev->dev->phys_caps.base_proxy_sqpn) / 8;
1497 	if (slave != ctx->slave) {
1498 		mlx4_ib_warn(ctx->ib_dev, "can't multiplex bad sqp:%d: "
1499 			     "belongs to another slave\n", wc->src_qp);
1500 		return;
1501 	}
1502 
1503 	/* Map transaction ID */
1504 	ib_dma_sync_single_for_cpu(ctx->ib_dev, tun_qp->ring[wr_ix].map,
1505 				   sizeof (struct mlx4_tunnel_mad),
1506 				   DMA_FROM_DEVICE);
1507 	switch (tunnel->mad.mad_hdr.method) {
1508 	case IB_MGMT_METHOD_SET:
1509 	case IB_MGMT_METHOD_GET:
1510 	case IB_MGMT_METHOD_REPORT:
1511 	case IB_SA_METHOD_GET_TABLE:
1512 	case IB_SA_METHOD_DELETE:
1513 	case IB_SA_METHOD_GET_MULTI:
1514 	case IB_SA_METHOD_GET_TRACE_TBL:
1515 		slave_id = (u8 *) &tunnel->mad.mad_hdr.tid;
1516 		if (*slave_id) {
1517 			mlx4_ib_warn(ctx->ib_dev, "egress mad has non-null tid msb:%d "
1518 				     "class:%d slave:%d\n", *slave_id,
1519 				     tunnel->mad.mad_hdr.mgmt_class, slave);
1520 			return;
1521 		} else
1522 			*slave_id = slave;
1523 	default:
1524 		/* nothing */;
1525 	}
1526 
1527 	/* Class-specific handling */
1528 	switch (tunnel->mad.mad_hdr.mgmt_class) {
1529 	case IB_MGMT_CLASS_SUBN_LID_ROUTED:
1530 	case IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE:
1531 		if (slave != mlx4_master_func_num(dev->dev) &&
1532 		    !mlx4_vf_smi_enabled(dev->dev, slave, ctx->port))
1533 			return;
1534 		break;
1535 	case IB_MGMT_CLASS_SUBN_ADM:
1536 		if (mlx4_ib_multiplex_sa_handler(ctx->ib_dev, ctx->port, slave,
1537 			      (struct ib_sa_mad *) &tunnel->mad))
1538 			return;
1539 		break;
1540 	case IB_MGMT_CLASS_CM:
1541 		if (mlx4_ib_multiplex_cm_handler(ctx->ib_dev, ctx->port, slave,
1542 			      (struct ib_mad *) &tunnel->mad))
1543 			return;
1544 		break;
1545 	case IB_MGMT_CLASS_DEVICE_MGMT:
1546 		if (tunnel->mad.mad_hdr.method != IB_MGMT_METHOD_GET &&
1547 		    tunnel->mad.mad_hdr.method != IB_MGMT_METHOD_SET)
1548 			return;
1549 		break;
1550 	default:
1551 		/* Drop unsupported classes for slaves in tunnel mode */
1552 		if (slave != mlx4_master_func_num(dev->dev)) {
1553 			mlx4_ib_warn(ctx->ib_dev, "dropping unsupported egress mad from class:%d "
1554 				     "for slave:%d\n", tunnel->mad.mad_hdr.mgmt_class, slave);
1555 			return;
1556 		}
1557 	}
1558 
1559 	/* We are using standard ib_core services to send the mad, so generate a
1560 	 * stadard address handle by decoding the tunnelled mlx4_ah fields */
1561 	memcpy(&ah.av, &tunnel->hdr.av, sizeof (struct mlx4_av));
1562 	ah.ibah.device = ctx->ib_dev;
1563 
1564 	port = be32_to_cpu(ah.av.ib.port_pd) >> 24;
1565 	port = mlx4_slave_convert_port(dev->dev, slave, port);
1566 	if (port < 0)
1567 		return;
1568 	ah.av.ib.port_pd = cpu_to_be32(port << 24 | (be32_to_cpu(ah.av.ib.port_pd) & 0xffffff));
1569 	ah.ibah.type = rdma_ah_find_type(&dev->ib_dev, port);
1570 
1571 	mlx4_ib_query_ah(&ah.ibah, &ah_attr);
1572 	if (rdma_ah_get_ah_flags(&ah_attr) & IB_AH_GRH)
1573 		fill_in_real_sgid_index(dev, slave, ctx->port, &ah_attr);
1574 	dmac = rdma_ah_retrieve_dmac(&ah_attr);
1575 	if (dmac)
1576 		memcpy(dmac, tunnel->hdr.mac, ETH_ALEN);
1577 	vlan_id = be16_to_cpu(tunnel->hdr.vlan);
1578 	/* if slave have default vlan use it */
1579 	if (mlx4_get_slave_default_vlan(dev->dev, ctx->port, slave,
1580 					&vlan_id, &qos))
1581 		rdma_ah_set_sl(&ah_attr, qos);
1582 
1583 	mlx4_ib_send_to_wire(dev, slave, ctx->port,
1584 			     is_proxy_qp0(dev, wc->src_qp, slave) ?
1585 			     IB_QPT_SMI : IB_QPT_GSI,
1586 			     be16_to_cpu(tunnel->hdr.pkey_index),
1587 			     be32_to_cpu(tunnel->hdr.remote_qpn),
1588 			     be32_to_cpu(tunnel->hdr.qkey),
1589 			     &ah_attr, wc->smac, vlan_id, &tunnel->mad);
1590 }
1591 
1592 static int mlx4_ib_alloc_pv_bufs(struct mlx4_ib_demux_pv_ctx *ctx,
1593 				 enum ib_qp_type qp_type, int is_tun)
1594 {
1595 	int i;
1596 	struct mlx4_ib_demux_pv_qp *tun_qp;
1597 	int rx_buf_size, tx_buf_size;
1598 
1599 	if (qp_type > IB_QPT_GSI)
1600 		return -EINVAL;
1601 
1602 	tun_qp = &ctx->qp[qp_type];
1603 
1604 	tun_qp->ring = kcalloc(MLX4_NUM_TUNNEL_BUFS,
1605 			       sizeof(struct mlx4_ib_buf),
1606 			       GFP_KERNEL);
1607 	if (!tun_qp->ring)
1608 		return -ENOMEM;
1609 
1610 	tun_qp->tx_ring = kcalloc(MLX4_NUM_TUNNEL_BUFS,
1611 				  sizeof (struct mlx4_ib_tun_tx_buf),
1612 				  GFP_KERNEL);
1613 	if (!tun_qp->tx_ring) {
1614 		kfree(tun_qp->ring);
1615 		tun_qp->ring = NULL;
1616 		return -ENOMEM;
1617 	}
1618 
1619 	if (is_tun) {
1620 		rx_buf_size = sizeof (struct mlx4_tunnel_mad);
1621 		tx_buf_size = sizeof (struct mlx4_rcv_tunnel_mad);
1622 	} else {
1623 		rx_buf_size = sizeof (struct mlx4_mad_rcv_buf);
1624 		tx_buf_size = sizeof (struct mlx4_mad_snd_buf);
1625 	}
1626 
1627 	for (i = 0; i < MLX4_NUM_TUNNEL_BUFS; i++) {
1628 		tun_qp->ring[i].addr = kmalloc(rx_buf_size, GFP_KERNEL);
1629 		if (!tun_qp->ring[i].addr)
1630 			goto err;
1631 		tun_qp->ring[i].map = ib_dma_map_single(ctx->ib_dev,
1632 							tun_qp->ring[i].addr,
1633 							rx_buf_size,
1634 							DMA_FROM_DEVICE);
1635 		if (ib_dma_mapping_error(ctx->ib_dev, tun_qp->ring[i].map)) {
1636 			kfree(tun_qp->ring[i].addr);
1637 			goto err;
1638 		}
1639 	}
1640 
1641 	for (i = 0; i < MLX4_NUM_TUNNEL_BUFS; i++) {
1642 		tun_qp->tx_ring[i].buf.addr =
1643 			kmalloc(tx_buf_size, GFP_KERNEL);
1644 		if (!tun_qp->tx_ring[i].buf.addr)
1645 			goto tx_err;
1646 		tun_qp->tx_ring[i].buf.map =
1647 			ib_dma_map_single(ctx->ib_dev,
1648 					  tun_qp->tx_ring[i].buf.addr,
1649 					  tx_buf_size,
1650 					  DMA_TO_DEVICE);
1651 		if (ib_dma_mapping_error(ctx->ib_dev,
1652 					 tun_qp->tx_ring[i].buf.map)) {
1653 			kfree(tun_qp->tx_ring[i].buf.addr);
1654 			goto tx_err;
1655 		}
1656 		tun_qp->tx_ring[i].ah = NULL;
1657 	}
1658 	spin_lock_init(&tun_qp->tx_lock);
1659 	tun_qp->tx_ix_head = 0;
1660 	tun_qp->tx_ix_tail = 0;
1661 	tun_qp->proxy_qpt = qp_type;
1662 
1663 	return 0;
1664 
1665 tx_err:
1666 	while (i > 0) {
1667 		--i;
1668 		ib_dma_unmap_single(ctx->ib_dev, tun_qp->tx_ring[i].buf.map,
1669 				    tx_buf_size, DMA_TO_DEVICE);
1670 		kfree(tun_qp->tx_ring[i].buf.addr);
1671 	}
1672 	i = MLX4_NUM_TUNNEL_BUFS;
1673 err:
1674 	while (i > 0) {
1675 		--i;
1676 		ib_dma_unmap_single(ctx->ib_dev, tun_qp->ring[i].map,
1677 				    rx_buf_size, DMA_FROM_DEVICE);
1678 		kfree(tun_qp->ring[i].addr);
1679 	}
1680 	kfree(tun_qp->tx_ring);
1681 	tun_qp->tx_ring = NULL;
1682 	kfree(tun_qp->ring);
1683 	tun_qp->ring = NULL;
1684 	return -ENOMEM;
1685 }
1686 
1687 static void mlx4_ib_free_pv_qp_bufs(struct mlx4_ib_demux_pv_ctx *ctx,
1688 				     enum ib_qp_type qp_type, int is_tun)
1689 {
1690 	int i;
1691 	struct mlx4_ib_demux_pv_qp *tun_qp;
1692 	int rx_buf_size, tx_buf_size;
1693 
1694 	if (qp_type > IB_QPT_GSI)
1695 		return;
1696 
1697 	tun_qp = &ctx->qp[qp_type];
1698 	if (is_tun) {
1699 		rx_buf_size = sizeof (struct mlx4_tunnel_mad);
1700 		tx_buf_size = sizeof (struct mlx4_rcv_tunnel_mad);
1701 	} else {
1702 		rx_buf_size = sizeof (struct mlx4_mad_rcv_buf);
1703 		tx_buf_size = sizeof (struct mlx4_mad_snd_buf);
1704 	}
1705 
1706 
1707 	for (i = 0; i < MLX4_NUM_TUNNEL_BUFS; i++) {
1708 		ib_dma_unmap_single(ctx->ib_dev, tun_qp->ring[i].map,
1709 				    rx_buf_size, DMA_FROM_DEVICE);
1710 		kfree(tun_qp->ring[i].addr);
1711 	}
1712 
1713 	for (i = 0; i < MLX4_NUM_TUNNEL_BUFS; i++) {
1714 		ib_dma_unmap_single(ctx->ib_dev, tun_qp->tx_ring[i].buf.map,
1715 				    tx_buf_size, DMA_TO_DEVICE);
1716 		kfree(tun_qp->tx_ring[i].buf.addr);
1717 		if (tun_qp->tx_ring[i].ah)
1718 			rdma_destroy_ah(tun_qp->tx_ring[i].ah, 0);
1719 	}
1720 	kfree(tun_qp->tx_ring);
1721 	kfree(tun_qp->ring);
1722 }
1723 
1724 static void mlx4_ib_tunnel_comp_worker(struct work_struct *work)
1725 {
1726 	struct mlx4_ib_demux_pv_ctx *ctx;
1727 	struct mlx4_ib_demux_pv_qp *tun_qp;
1728 	struct ib_wc wc;
1729 	int ret;
1730 	ctx = container_of(work, struct mlx4_ib_demux_pv_ctx, work);
1731 	ib_req_notify_cq(ctx->cq, IB_CQ_NEXT_COMP);
1732 
1733 	while (ib_poll_cq(ctx->cq, 1, &wc) == 1) {
1734 		tun_qp = &ctx->qp[MLX4_TUN_WRID_QPN(wc.wr_id)];
1735 		if (wc.status == IB_WC_SUCCESS) {
1736 			switch (wc.opcode) {
1737 			case IB_WC_RECV:
1738 				mlx4_ib_multiplex_mad(ctx, &wc);
1739 				ret = mlx4_ib_post_pv_qp_buf(ctx, tun_qp,
1740 							     wc.wr_id &
1741 							     (MLX4_NUM_TUNNEL_BUFS - 1));
1742 				if (ret)
1743 					pr_err("Failed reposting tunnel "
1744 					       "buf:%lld\n", wc.wr_id);
1745 				break;
1746 			case IB_WC_SEND:
1747 				pr_debug("received tunnel send completion:"
1748 					 "wrid=0x%llx, status=0x%x\n",
1749 					 wc.wr_id, wc.status);
1750 				rdma_destroy_ah(tun_qp->tx_ring[wc.wr_id &
1751 					      (MLX4_NUM_TUNNEL_BUFS - 1)].ah, 0);
1752 				tun_qp->tx_ring[wc.wr_id & (MLX4_NUM_TUNNEL_BUFS - 1)].ah
1753 					= NULL;
1754 				spin_lock(&tun_qp->tx_lock);
1755 				tun_qp->tx_ix_tail++;
1756 				spin_unlock(&tun_qp->tx_lock);
1757 
1758 				break;
1759 			default:
1760 				break;
1761 			}
1762 		} else  {
1763 			pr_debug("mlx4_ib: completion error in tunnel: %d."
1764 				 " status = %d, wrid = 0x%llx\n",
1765 				 ctx->slave, wc.status, wc.wr_id);
1766 			if (!MLX4_TUN_IS_RECV(wc.wr_id)) {
1767 				rdma_destroy_ah(tun_qp->tx_ring[wc.wr_id &
1768 					      (MLX4_NUM_TUNNEL_BUFS - 1)].ah, 0);
1769 				tun_qp->tx_ring[wc.wr_id & (MLX4_NUM_TUNNEL_BUFS - 1)].ah
1770 					= NULL;
1771 				spin_lock(&tun_qp->tx_lock);
1772 				tun_qp->tx_ix_tail++;
1773 				spin_unlock(&tun_qp->tx_lock);
1774 			}
1775 		}
1776 	}
1777 }
1778 
1779 static void pv_qp_event_handler(struct ib_event *event, void *qp_context)
1780 {
1781 	struct mlx4_ib_demux_pv_ctx *sqp = qp_context;
1782 
1783 	/* It's worse than that! He's dead, Jim! */
1784 	pr_err("Fatal error (%d) on a MAD QP on port %d\n",
1785 	       event->event, sqp->port);
1786 }
1787 
1788 static int create_pv_sqp(struct mlx4_ib_demux_pv_ctx *ctx,
1789 			    enum ib_qp_type qp_type, int create_tun)
1790 {
1791 	int i, ret;
1792 	struct mlx4_ib_demux_pv_qp *tun_qp;
1793 	struct mlx4_ib_qp_tunnel_init_attr qp_init_attr;
1794 	struct ib_qp_attr attr;
1795 	int qp_attr_mask_INIT;
1796 
1797 	if (qp_type > IB_QPT_GSI)
1798 		return -EINVAL;
1799 
1800 	tun_qp = &ctx->qp[qp_type];
1801 
1802 	memset(&qp_init_attr, 0, sizeof qp_init_attr);
1803 	qp_init_attr.init_attr.send_cq = ctx->cq;
1804 	qp_init_attr.init_attr.recv_cq = ctx->cq;
1805 	qp_init_attr.init_attr.sq_sig_type = IB_SIGNAL_ALL_WR;
1806 	qp_init_attr.init_attr.cap.max_send_wr = MLX4_NUM_TUNNEL_BUFS;
1807 	qp_init_attr.init_attr.cap.max_recv_wr = MLX4_NUM_TUNNEL_BUFS;
1808 	qp_init_attr.init_attr.cap.max_send_sge = 1;
1809 	qp_init_attr.init_attr.cap.max_recv_sge = 1;
1810 	if (create_tun) {
1811 		qp_init_attr.init_attr.qp_type = IB_QPT_UD;
1812 		qp_init_attr.init_attr.create_flags = MLX4_IB_SRIOV_TUNNEL_QP;
1813 		qp_init_attr.port = ctx->port;
1814 		qp_init_attr.slave = ctx->slave;
1815 		qp_init_attr.proxy_qp_type = qp_type;
1816 		qp_attr_mask_INIT = IB_QP_STATE | IB_QP_PKEY_INDEX |
1817 			   IB_QP_QKEY | IB_QP_PORT;
1818 	} else {
1819 		qp_init_attr.init_attr.qp_type = qp_type;
1820 		qp_init_attr.init_attr.create_flags = MLX4_IB_SRIOV_SQP;
1821 		qp_attr_mask_INIT = IB_QP_STATE | IB_QP_PKEY_INDEX | IB_QP_QKEY;
1822 	}
1823 	qp_init_attr.init_attr.port_num = ctx->port;
1824 	qp_init_attr.init_attr.qp_context = ctx;
1825 	qp_init_attr.init_attr.event_handler = pv_qp_event_handler;
1826 	tun_qp->qp = ib_create_qp(ctx->pd, &qp_init_attr.init_attr);
1827 	if (IS_ERR(tun_qp->qp)) {
1828 		ret = PTR_ERR(tun_qp->qp);
1829 		tun_qp->qp = NULL;
1830 		pr_err("Couldn't create %s QP (%d)\n",
1831 		       create_tun ? "tunnel" : "special", ret);
1832 		return ret;
1833 	}
1834 
1835 	memset(&attr, 0, sizeof attr);
1836 	attr.qp_state = IB_QPS_INIT;
1837 	ret = 0;
1838 	if (create_tun)
1839 		ret = find_slave_port_pkey_ix(to_mdev(ctx->ib_dev), ctx->slave,
1840 					      ctx->port, IB_DEFAULT_PKEY_FULL,
1841 					      &attr.pkey_index);
1842 	if (ret || !create_tun)
1843 		attr.pkey_index =
1844 			to_mdev(ctx->ib_dev)->pkeys.virt2phys_pkey[ctx->slave][ctx->port - 1][0];
1845 	attr.qkey = IB_QP1_QKEY;
1846 	attr.port_num = ctx->port;
1847 	ret = ib_modify_qp(tun_qp->qp, &attr, qp_attr_mask_INIT);
1848 	if (ret) {
1849 		pr_err("Couldn't change %s qp state to INIT (%d)\n",
1850 		       create_tun ? "tunnel" : "special", ret);
1851 		goto err_qp;
1852 	}
1853 	attr.qp_state = IB_QPS_RTR;
1854 	ret = ib_modify_qp(tun_qp->qp, &attr, IB_QP_STATE);
1855 	if (ret) {
1856 		pr_err("Couldn't change %s qp state to RTR (%d)\n",
1857 		       create_tun ? "tunnel" : "special", ret);
1858 		goto err_qp;
1859 	}
1860 	attr.qp_state = IB_QPS_RTS;
1861 	attr.sq_psn = 0;
1862 	ret = ib_modify_qp(tun_qp->qp, &attr, IB_QP_STATE | IB_QP_SQ_PSN);
1863 	if (ret) {
1864 		pr_err("Couldn't change %s qp state to RTS (%d)\n",
1865 		       create_tun ? "tunnel" : "special", ret);
1866 		goto err_qp;
1867 	}
1868 
1869 	for (i = 0; i < MLX4_NUM_TUNNEL_BUFS; i++) {
1870 		ret = mlx4_ib_post_pv_qp_buf(ctx, tun_qp, i);
1871 		if (ret) {
1872 			pr_err(" mlx4_ib_post_pv_buf error"
1873 			       " (err = %d, i = %d)\n", ret, i);
1874 			goto err_qp;
1875 		}
1876 	}
1877 	return 0;
1878 
1879 err_qp:
1880 	ib_destroy_qp(tun_qp->qp);
1881 	tun_qp->qp = NULL;
1882 	return ret;
1883 }
1884 
1885 /*
1886  * IB MAD completion callback for real SQPs
1887  */
1888 static void mlx4_ib_sqp_comp_worker(struct work_struct *work)
1889 {
1890 	struct mlx4_ib_demux_pv_ctx *ctx;
1891 	struct mlx4_ib_demux_pv_qp *sqp;
1892 	struct ib_wc wc;
1893 	struct ib_grh *grh;
1894 	struct ib_mad *mad;
1895 
1896 	ctx = container_of(work, struct mlx4_ib_demux_pv_ctx, work);
1897 	ib_req_notify_cq(ctx->cq, IB_CQ_NEXT_COMP);
1898 
1899 	while (mlx4_ib_poll_cq(ctx->cq, 1, &wc) == 1) {
1900 		sqp = &ctx->qp[MLX4_TUN_WRID_QPN(wc.wr_id)];
1901 		if (wc.status == IB_WC_SUCCESS) {
1902 			switch (wc.opcode) {
1903 			case IB_WC_SEND:
1904 				kfree(sqp->tx_ring[wc.wr_id &
1905 				      (MLX4_NUM_TUNNEL_BUFS - 1)].ah);
1906 				sqp->tx_ring[wc.wr_id & (MLX4_NUM_TUNNEL_BUFS - 1)].ah
1907 					= NULL;
1908 				spin_lock(&sqp->tx_lock);
1909 				sqp->tx_ix_tail++;
1910 				spin_unlock(&sqp->tx_lock);
1911 				break;
1912 			case IB_WC_RECV:
1913 				mad = (struct ib_mad *) &(((struct mlx4_mad_rcv_buf *)
1914 						(sqp->ring[wc.wr_id &
1915 						(MLX4_NUM_TUNNEL_BUFS - 1)].addr))->payload);
1916 				grh = &(((struct mlx4_mad_rcv_buf *)
1917 						(sqp->ring[wc.wr_id &
1918 						(MLX4_NUM_TUNNEL_BUFS - 1)].addr))->grh);
1919 				mlx4_ib_demux_mad(ctx->ib_dev, ctx->port, &wc, grh, mad);
1920 				if (mlx4_ib_post_pv_qp_buf(ctx, sqp, wc.wr_id &
1921 							   (MLX4_NUM_TUNNEL_BUFS - 1)))
1922 					pr_err("Failed reposting SQP "
1923 					       "buf:%lld\n", wc.wr_id);
1924 				break;
1925 			default:
1926 				break;
1927 			}
1928 		} else  {
1929 			pr_debug("mlx4_ib: completion error in tunnel: %d."
1930 				 " status = %d, wrid = 0x%llx\n",
1931 				 ctx->slave, wc.status, wc.wr_id);
1932 			if (!MLX4_TUN_IS_RECV(wc.wr_id)) {
1933 				kfree(sqp->tx_ring[wc.wr_id &
1934 				      (MLX4_NUM_TUNNEL_BUFS - 1)].ah);
1935 				sqp->tx_ring[wc.wr_id & (MLX4_NUM_TUNNEL_BUFS - 1)].ah
1936 					= NULL;
1937 				spin_lock(&sqp->tx_lock);
1938 				sqp->tx_ix_tail++;
1939 				spin_unlock(&sqp->tx_lock);
1940 			}
1941 		}
1942 	}
1943 }
1944 
1945 static int alloc_pv_object(struct mlx4_ib_dev *dev, int slave, int port,
1946 			       struct mlx4_ib_demux_pv_ctx **ret_ctx)
1947 {
1948 	struct mlx4_ib_demux_pv_ctx *ctx;
1949 
1950 	*ret_ctx = NULL;
1951 	ctx = kzalloc(sizeof (struct mlx4_ib_demux_pv_ctx), GFP_KERNEL);
1952 	if (!ctx)
1953 		return -ENOMEM;
1954 
1955 	ctx->ib_dev = &dev->ib_dev;
1956 	ctx->port = port;
1957 	ctx->slave = slave;
1958 	*ret_ctx = ctx;
1959 	return 0;
1960 }
1961 
1962 static void free_pv_object(struct mlx4_ib_dev *dev, int slave, int port)
1963 {
1964 	if (dev->sriov.demux[port - 1].tun[slave]) {
1965 		kfree(dev->sriov.demux[port - 1].tun[slave]);
1966 		dev->sriov.demux[port - 1].tun[slave] = NULL;
1967 	}
1968 }
1969 
1970 static int create_pv_resources(struct ib_device *ibdev, int slave, int port,
1971 			       int create_tun, struct mlx4_ib_demux_pv_ctx *ctx)
1972 {
1973 	int ret, cq_size;
1974 	struct ib_cq_init_attr cq_attr = {};
1975 
1976 	if (ctx->state != DEMUX_PV_STATE_DOWN)
1977 		return -EEXIST;
1978 
1979 	ctx->state = DEMUX_PV_STATE_STARTING;
1980 	/* have QP0 only if link layer is IB */
1981 	if (rdma_port_get_link_layer(ibdev, ctx->port) ==
1982 	    IB_LINK_LAYER_INFINIBAND)
1983 		ctx->has_smi = 1;
1984 
1985 	if (ctx->has_smi) {
1986 		ret = mlx4_ib_alloc_pv_bufs(ctx, IB_QPT_SMI, create_tun);
1987 		if (ret) {
1988 			pr_err("Failed allocating qp0 tunnel bufs (%d)\n", ret);
1989 			goto err_out;
1990 		}
1991 	}
1992 
1993 	ret = mlx4_ib_alloc_pv_bufs(ctx, IB_QPT_GSI, create_tun);
1994 	if (ret) {
1995 		pr_err("Failed allocating qp1 tunnel bufs (%d)\n", ret);
1996 		goto err_out_qp0;
1997 	}
1998 
1999 	cq_size = 2 * MLX4_NUM_TUNNEL_BUFS;
2000 	if (ctx->has_smi)
2001 		cq_size *= 2;
2002 
2003 	cq_attr.cqe = cq_size;
2004 	ctx->cq = ib_create_cq(ctx->ib_dev, mlx4_ib_tunnel_comp_handler,
2005 			       NULL, ctx, &cq_attr);
2006 	if (IS_ERR(ctx->cq)) {
2007 		ret = PTR_ERR(ctx->cq);
2008 		pr_err("Couldn't create tunnel CQ (%d)\n", ret);
2009 		goto err_buf;
2010 	}
2011 
2012 	ctx->pd = ib_alloc_pd(ctx->ib_dev, 0);
2013 	if (IS_ERR(ctx->pd)) {
2014 		ret = PTR_ERR(ctx->pd);
2015 		pr_err("Couldn't create tunnel PD (%d)\n", ret);
2016 		goto err_cq;
2017 	}
2018 
2019 	if (ctx->has_smi) {
2020 		ret = create_pv_sqp(ctx, IB_QPT_SMI, create_tun);
2021 		if (ret) {
2022 			pr_err("Couldn't create %s QP0 (%d)\n",
2023 			       create_tun ? "tunnel for" : "",  ret);
2024 			goto err_pd;
2025 		}
2026 	}
2027 
2028 	ret = create_pv_sqp(ctx, IB_QPT_GSI, create_tun);
2029 	if (ret) {
2030 		pr_err("Couldn't create %s QP1 (%d)\n",
2031 		       create_tun ? "tunnel for" : "",  ret);
2032 		goto err_qp0;
2033 	}
2034 
2035 	if (create_tun)
2036 		INIT_WORK(&ctx->work, mlx4_ib_tunnel_comp_worker);
2037 	else
2038 		INIT_WORK(&ctx->work, mlx4_ib_sqp_comp_worker);
2039 
2040 	ctx->wq = to_mdev(ibdev)->sriov.demux[port - 1].wq;
2041 
2042 	ret = ib_req_notify_cq(ctx->cq, IB_CQ_NEXT_COMP);
2043 	if (ret) {
2044 		pr_err("Couldn't arm tunnel cq (%d)\n", ret);
2045 		goto err_wq;
2046 	}
2047 	ctx->state = DEMUX_PV_STATE_ACTIVE;
2048 	return 0;
2049 
2050 err_wq:
2051 	ctx->wq = NULL;
2052 	ib_destroy_qp(ctx->qp[1].qp);
2053 	ctx->qp[1].qp = NULL;
2054 
2055 
2056 err_qp0:
2057 	if (ctx->has_smi)
2058 		ib_destroy_qp(ctx->qp[0].qp);
2059 	ctx->qp[0].qp = NULL;
2060 
2061 err_pd:
2062 	ib_dealloc_pd(ctx->pd);
2063 	ctx->pd = NULL;
2064 
2065 err_cq:
2066 	ib_destroy_cq(ctx->cq);
2067 	ctx->cq = NULL;
2068 
2069 err_buf:
2070 	mlx4_ib_free_pv_qp_bufs(ctx, IB_QPT_GSI, create_tun);
2071 
2072 err_out_qp0:
2073 	if (ctx->has_smi)
2074 		mlx4_ib_free_pv_qp_bufs(ctx, IB_QPT_SMI, create_tun);
2075 err_out:
2076 	ctx->state = DEMUX_PV_STATE_DOWN;
2077 	return ret;
2078 }
2079 
2080 static void destroy_pv_resources(struct mlx4_ib_dev *dev, int slave, int port,
2081 				 struct mlx4_ib_demux_pv_ctx *ctx, int flush)
2082 {
2083 	if (!ctx)
2084 		return;
2085 	if (ctx->state > DEMUX_PV_STATE_DOWN) {
2086 		ctx->state = DEMUX_PV_STATE_DOWNING;
2087 		if (flush)
2088 			flush_workqueue(ctx->wq);
2089 		if (ctx->has_smi) {
2090 			ib_destroy_qp(ctx->qp[0].qp);
2091 			ctx->qp[0].qp = NULL;
2092 			mlx4_ib_free_pv_qp_bufs(ctx, IB_QPT_SMI, 1);
2093 		}
2094 		ib_destroy_qp(ctx->qp[1].qp);
2095 		ctx->qp[1].qp = NULL;
2096 		mlx4_ib_free_pv_qp_bufs(ctx, IB_QPT_GSI, 1);
2097 		ib_dealloc_pd(ctx->pd);
2098 		ctx->pd = NULL;
2099 		ib_destroy_cq(ctx->cq);
2100 		ctx->cq = NULL;
2101 		ctx->state = DEMUX_PV_STATE_DOWN;
2102 	}
2103 }
2104 
2105 static int mlx4_ib_tunnels_update(struct mlx4_ib_dev *dev, int slave,
2106 				  int port, int do_init)
2107 {
2108 	int ret = 0;
2109 
2110 	if (!do_init) {
2111 		clean_vf_mcast(&dev->sriov.demux[port - 1], slave);
2112 		/* for master, destroy real sqp resources */
2113 		if (slave == mlx4_master_func_num(dev->dev))
2114 			destroy_pv_resources(dev, slave, port,
2115 					     dev->sriov.sqps[port - 1], 1);
2116 		/* destroy the tunnel qp resources */
2117 		destroy_pv_resources(dev, slave, port,
2118 				     dev->sriov.demux[port - 1].tun[slave], 1);
2119 		return 0;
2120 	}
2121 
2122 	/* create the tunnel qp resources */
2123 	ret = create_pv_resources(&dev->ib_dev, slave, port, 1,
2124 				  dev->sriov.demux[port - 1].tun[slave]);
2125 
2126 	/* for master, create the real sqp resources */
2127 	if (!ret && slave == mlx4_master_func_num(dev->dev))
2128 		ret = create_pv_resources(&dev->ib_dev, slave, port, 0,
2129 					  dev->sriov.sqps[port - 1]);
2130 	return ret;
2131 }
2132 
2133 void mlx4_ib_tunnels_update_work(struct work_struct *work)
2134 {
2135 	struct mlx4_ib_demux_work *dmxw;
2136 
2137 	dmxw = container_of(work, struct mlx4_ib_demux_work, work);
2138 	mlx4_ib_tunnels_update(dmxw->dev, dmxw->slave, (int) dmxw->port,
2139 			       dmxw->do_init);
2140 	kfree(dmxw);
2141 	return;
2142 }
2143 
2144 static int mlx4_ib_alloc_demux_ctx(struct mlx4_ib_dev *dev,
2145 				       struct mlx4_ib_demux_ctx *ctx,
2146 				       int port)
2147 {
2148 	char name[12];
2149 	int ret = 0;
2150 	int i;
2151 
2152 	ctx->tun = kcalloc(dev->dev->caps.sqp_demux,
2153 			   sizeof (struct mlx4_ib_demux_pv_ctx *), GFP_KERNEL);
2154 	if (!ctx->tun)
2155 		return -ENOMEM;
2156 
2157 	ctx->dev = dev;
2158 	ctx->port = port;
2159 	ctx->ib_dev = &dev->ib_dev;
2160 
2161 	for (i = 0;
2162 	     i < min(dev->dev->caps.sqp_demux,
2163 	     (u16)(dev->dev->persist->num_vfs + 1));
2164 	     i++) {
2165 		struct mlx4_active_ports actv_ports =
2166 			mlx4_get_active_ports(dev->dev, i);
2167 
2168 		if (!test_bit(port - 1, actv_ports.ports))
2169 			continue;
2170 
2171 		ret = alloc_pv_object(dev, i, port, &ctx->tun[i]);
2172 		if (ret) {
2173 			ret = -ENOMEM;
2174 			goto err_mcg;
2175 		}
2176 	}
2177 
2178 	ret = mlx4_ib_mcg_port_init(ctx);
2179 	if (ret) {
2180 		pr_err("Failed initializing mcg para-virt (%d)\n", ret);
2181 		goto err_mcg;
2182 	}
2183 
2184 	snprintf(name, sizeof name, "mlx4_ibt%d", port);
2185 	ctx->wq = alloc_ordered_workqueue(name, WQ_MEM_RECLAIM);
2186 	if (!ctx->wq) {
2187 		pr_err("Failed to create tunnelling WQ for port %d\n", port);
2188 		ret = -ENOMEM;
2189 		goto err_wq;
2190 	}
2191 
2192 	snprintf(name, sizeof name, "mlx4_ibud%d", port);
2193 	ctx->ud_wq = alloc_ordered_workqueue(name, WQ_MEM_RECLAIM);
2194 	if (!ctx->ud_wq) {
2195 		pr_err("Failed to create up/down WQ for port %d\n", port);
2196 		ret = -ENOMEM;
2197 		goto err_udwq;
2198 	}
2199 
2200 	return 0;
2201 
2202 err_udwq:
2203 	destroy_workqueue(ctx->wq);
2204 	ctx->wq = NULL;
2205 
2206 err_wq:
2207 	mlx4_ib_mcg_port_cleanup(ctx, 1);
2208 err_mcg:
2209 	for (i = 0; i < dev->dev->caps.sqp_demux; i++)
2210 		free_pv_object(dev, i, port);
2211 	kfree(ctx->tun);
2212 	ctx->tun = NULL;
2213 	return ret;
2214 }
2215 
2216 static void mlx4_ib_free_sqp_ctx(struct mlx4_ib_demux_pv_ctx *sqp_ctx)
2217 {
2218 	if (sqp_ctx->state > DEMUX_PV_STATE_DOWN) {
2219 		sqp_ctx->state = DEMUX_PV_STATE_DOWNING;
2220 		flush_workqueue(sqp_ctx->wq);
2221 		if (sqp_ctx->has_smi) {
2222 			ib_destroy_qp(sqp_ctx->qp[0].qp);
2223 			sqp_ctx->qp[0].qp = NULL;
2224 			mlx4_ib_free_pv_qp_bufs(sqp_ctx, IB_QPT_SMI, 0);
2225 		}
2226 		ib_destroy_qp(sqp_ctx->qp[1].qp);
2227 		sqp_ctx->qp[1].qp = NULL;
2228 		mlx4_ib_free_pv_qp_bufs(sqp_ctx, IB_QPT_GSI, 0);
2229 		ib_dealloc_pd(sqp_ctx->pd);
2230 		sqp_ctx->pd = NULL;
2231 		ib_destroy_cq(sqp_ctx->cq);
2232 		sqp_ctx->cq = NULL;
2233 		sqp_ctx->state = DEMUX_PV_STATE_DOWN;
2234 	}
2235 }
2236 
2237 static void mlx4_ib_free_demux_ctx(struct mlx4_ib_demux_ctx *ctx)
2238 {
2239 	int i;
2240 	if (ctx) {
2241 		struct mlx4_ib_dev *dev = to_mdev(ctx->ib_dev);
2242 		mlx4_ib_mcg_port_cleanup(ctx, 1);
2243 		for (i = 0; i < dev->dev->caps.sqp_demux; i++) {
2244 			if (!ctx->tun[i])
2245 				continue;
2246 			if (ctx->tun[i]->state > DEMUX_PV_STATE_DOWN)
2247 				ctx->tun[i]->state = DEMUX_PV_STATE_DOWNING;
2248 		}
2249 		flush_workqueue(ctx->wq);
2250 		for (i = 0; i < dev->dev->caps.sqp_demux; i++) {
2251 			destroy_pv_resources(dev, i, ctx->port, ctx->tun[i], 0);
2252 			free_pv_object(dev, i, ctx->port);
2253 		}
2254 		kfree(ctx->tun);
2255 		destroy_workqueue(ctx->ud_wq);
2256 		destroy_workqueue(ctx->wq);
2257 	}
2258 }
2259 
2260 static void mlx4_ib_master_tunnels(struct mlx4_ib_dev *dev, int do_init)
2261 {
2262 	int i;
2263 
2264 	if (!mlx4_is_master(dev->dev))
2265 		return;
2266 	/* initialize or tear down tunnel QPs for the master */
2267 	for (i = 0; i < dev->dev->caps.num_ports; i++)
2268 		mlx4_ib_tunnels_update(dev, mlx4_master_func_num(dev->dev), i + 1, do_init);
2269 	return;
2270 }
2271 
2272 int mlx4_ib_init_sriov(struct mlx4_ib_dev *dev)
2273 {
2274 	int i = 0;
2275 	int err;
2276 
2277 	if (!mlx4_is_mfunc(dev->dev))
2278 		return 0;
2279 
2280 	dev->sriov.is_going_down = 0;
2281 	spin_lock_init(&dev->sriov.going_down_lock);
2282 	mlx4_ib_cm_paravirt_init(dev);
2283 
2284 	mlx4_ib_warn(&dev->ib_dev, "multi-function enabled\n");
2285 
2286 	if (mlx4_is_slave(dev->dev)) {
2287 		mlx4_ib_warn(&dev->ib_dev, "operating in qp1 tunnel mode\n");
2288 		return 0;
2289 	}
2290 
2291 	for (i = 0; i < dev->dev->caps.sqp_demux; i++) {
2292 		if (i == mlx4_master_func_num(dev->dev))
2293 			mlx4_put_slave_node_guid(dev->dev, i, dev->ib_dev.node_guid);
2294 		else
2295 			mlx4_put_slave_node_guid(dev->dev, i, mlx4_ib_gen_node_guid());
2296 	}
2297 
2298 	err = mlx4_ib_init_alias_guid_service(dev);
2299 	if (err) {
2300 		mlx4_ib_warn(&dev->ib_dev, "Failed init alias guid process.\n");
2301 		goto paravirt_err;
2302 	}
2303 	err = mlx4_ib_device_register_sysfs(dev);
2304 	if (err) {
2305 		mlx4_ib_warn(&dev->ib_dev, "Failed to register sysfs\n");
2306 		goto sysfs_err;
2307 	}
2308 
2309 	mlx4_ib_warn(&dev->ib_dev, "initializing demux service for %d qp1 clients\n",
2310 		     dev->dev->caps.sqp_demux);
2311 	for (i = 0; i < dev->num_ports; i++) {
2312 		union ib_gid gid;
2313 		err = __mlx4_ib_query_gid(&dev->ib_dev, i + 1, 0, &gid, 1);
2314 		if (err)
2315 			goto demux_err;
2316 		dev->sriov.demux[i].guid_cache[0] = gid.global.interface_id;
2317 		atomic64_set(&dev->sriov.demux[i].subnet_prefix,
2318 			     be64_to_cpu(gid.global.subnet_prefix));
2319 		err = alloc_pv_object(dev, mlx4_master_func_num(dev->dev), i + 1,
2320 				      &dev->sriov.sqps[i]);
2321 		if (err)
2322 			goto demux_err;
2323 		err = mlx4_ib_alloc_demux_ctx(dev, &dev->sriov.demux[i], i + 1);
2324 		if (err)
2325 			goto free_pv;
2326 	}
2327 	mlx4_ib_master_tunnels(dev, 1);
2328 	return 0;
2329 
2330 free_pv:
2331 	free_pv_object(dev, mlx4_master_func_num(dev->dev), i + 1);
2332 demux_err:
2333 	while (--i >= 0) {
2334 		free_pv_object(dev, mlx4_master_func_num(dev->dev), i + 1);
2335 		mlx4_ib_free_demux_ctx(&dev->sriov.demux[i]);
2336 	}
2337 	mlx4_ib_device_unregister_sysfs(dev);
2338 
2339 sysfs_err:
2340 	mlx4_ib_destroy_alias_guid_service(dev);
2341 
2342 paravirt_err:
2343 	mlx4_ib_cm_paravirt_clean(dev, -1);
2344 
2345 	return err;
2346 }
2347 
2348 void mlx4_ib_close_sriov(struct mlx4_ib_dev *dev)
2349 {
2350 	int i;
2351 	unsigned long flags;
2352 
2353 	if (!mlx4_is_mfunc(dev->dev))
2354 		return;
2355 
2356 	spin_lock_irqsave(&dev->sriov.going_down_lock, flags);
2357 	dev->sriov.is_going_down = 1;
2358 	spin_unlock_irqrestore(&dev->sriov.going_down_lock, flags);
2359 	if (mlx4_is_master(dev->dev)) {
2360 		for (i = 0; i < dev->num_ports; i++) {
2361 			flush_workqueue(dev->sriov.demux[i].ud_wq);
2362 			mlx4_ib_free_sqp_ctx(dev->sriov.sqps[i]);
2363 			kfree(dev->sriov.sqps[i]);
2364 			dev->sriov.sqps[i] = NULL;
2365 			mlx4_ib_free_demux_ctx(&dev->sriov.demux[i]);
2366 		}
2367 
2368 		mlx4_ib_cm_paravirt_clean(dev, -1);
2369 		mlx4_ib_destroy_alias_guid_service(dev);
2370 		mlx4_ib_device_unregister_sysfs(dev);
2371 	}
2372 }
2373