[PATCH] RDMA/nes: IB_QPT_RAW_PACKET QP type support for nes driver

[miroslaw.walukiewicz-ral2JQCrhuEAvxtiuMwx3w@public.gmane.org]

This patch adds a IB_QPT_RAW_PACKET QP type implementation for nes driver 
for receive and transmit of L2 Ethernet packets. 

Receiving of IPV4 IP mulicasts is supported also 
by ibv_attach_mcast/ibv_detach_mcast interface. 

This patch is based on following patch series:
[PATCH V1 0/4] New RAW_PACKET QP type
[PATCH V1 1/4] Rename RAW_ETY to RAW_ETHERTYPE
[PATCH V1 2/4]  New RAW_PACKET QP  type definition
[PATCH V1 3/4] Security check on QP type
[PATCH V1 4/4] Add RAW_PACKET to ib_attach/detach mcast calls

This patch should remove following patches from patchwork:
https://patchwork.kernel.org/patch/95988/
https://patchwork.kernel.org/patch/95915/

Signed-off-by: Mirek Walukiewicz <miroslaw.walukiewicz-ral2JQCrhuEAvxtiuMwx3w@public.gmane.org>
---

 drivers/infiniband/hw/nes/Makefile    |    2 
 drivers/infiniband/hw/nes/nes.c       |    4 
 drivers/infiniband/hw/nes/nes.h       |    2 
 drivers/infiniband/hw/nes/nes_nic.c   |   11 
 drivers/infiniband/hw/nes/nes_ud.c    | 2064 +++++++++++++++++++++++++++++++++
 drivers/infiniband/hw/nes/nes_ud.h    |   86 +
 drivers/infiniband/hw/nes/nes_verbs.c |  221 +++-
 drivers/infiniband/hw/nes/nes_verbs.h |    7 
 8 files changed, 2382 insertions(+), 15 deletions(-)
 create mode 100644 drivers/infiniband/hw/nes/nes_ud.c
 create mode 100644 drivers/infiniband/hw/nes/nes_ud.h


diff --git a/drivers/infiniband/hw/nes/Makefile b/drivers/infiniband/hw/nes/Makefile
index 3514851..850df8e 100644
--- a/drivers/infiniband/hw/nes/Makefile
+++ b/drivers/infiniband/hw/nes/Makefile
@@ -1,3 +1,3 @@
 obj-$(CONFIG_INFINIBAND_NES) += iw_nes.o
 
-iw_nes-objs := nes.o nes_hw.o nes_nic.o nes_utils.o nes_verbs.o nes_cm.o
+iw_nes-objs := nes.o nes_hw.o nes_nic.o nes_utils.o nes_verbs.o nes_cm.o nes_ud.o
diff --git a/drivers/infiniband/hw/nes/nes.c b/drivers/infiniband/hw/nes/nes.c
index de7b9d7..e430804 100644
--- a/drivers/infiniband/hw/nes/nes.c
+++ b/drivers/infiniband/hw/nes/nes.c
@@ -60,6 +60,8 @@
 #include <linux/route.h>
 #include <net/ip_fib.h>
 
+#include "nes_ud.h"
+
 MODULE_AUTHOR("NetEffect");
 MODULE_DESCRIPTION("NetEffect RNIC Low-level iWARP Driver");
 MODULE_LICENSE("Dual BSD/GPL");
@@ -1205,6 +1207,7 @@ static int __init nes_init_module(void)
 		if (retval1 < 0)
 			printk(KERN_ERR PFX "Unable to create NetEffect sys files.\n");
 	}
+	nes_ud_init();
 	return retval;
 }
 
@@ -1214,6 +1217,7 @@ static int __init nes_init_module(void)
  */
 static void __exit nes_exit_module(void)
 {
+	nes_ud_exit();
 	nes_cm_stop();
 	nes_remove_driver_sysfs(&nes_pci_driver);
 
diff --git a/drivers/infiniband/hw/nes/nes.h b/drivers/infiniband/hw/nes/nes.h
index cc78fee..faf420f 100644
--- a/drivers/infiniband/hw/nes/nes.h
+++ b/drivers/infiniband/hw/nes/nes.h
@@ -102,6 +102,7 @@
 #define NES_DRV_OPT_NO_INLINE_DATA       0x00000080
 #define NES_DRV_OPT_DISABLE_INT_MOD      0x00000100
 #define NES_DRV_OPT_DISABLE_VIRT_WQ      0x00000200
+#define NES_DRV_OPT_MCAST_LOGPORT_MAP    0x00000800
 
 #define NES_AEQ_EVENT_TIMEOUT         2500
 #define NES_DISCONNECT_EVENT_TIMEOUT  2000
@@ -128,6 +129,7 @@
 #define NES_DBG_IW_RX       0x00020000
 #define NES_DBG_IW_TX       0x00040000
 #define NES_DBG_SHUTDOWN    0x00080000
+#define NES_DBG_UD          0x00100000
 #define NES_DBG_RSVD1       0x10000000
 #define NES_DBG_RSVD2       0x20000000
 #define NES_DBG_RSVD3       0x40000000
diff --git a/drivers/infiniband/hw/nes/nes_nic.c b/drivers/infiniband/hw/nes/nes_nic.c
index 5cc0a9a..6743be1 100644
--- a/drivers/infiniband/hw/nes/nes_nic.c
+++ b/drivers/infiniband/hw/nes/nes_nic.c
@@ -896,7 +896,7 @@ static void nes_netdev_set_multicast_list(struct net_device *netdev)
 			((mc_nic_index = nesvnic->mcrq_mcast_filter(nesvnic,
 					get_addr(addrs, i++))) == 0));
 			if (mc_nic_index < 0)
-				mc_nic_index = nesvnic->nic_index;
+				mc_nic_index = (1 << nesvnic->nic_index);
 			while (nesadapter->pft_mcast_map[mc_index] < 16 &&
 				nesadapter->pft_mcast_map[mc_index] !=
 					nesvnic->nic_index &&
@@ -929,7 +929,7 @@ static void nes_netdev_set_multicast_list(struct net_device *netdev)
 				nes_write_indexed(nesdev,
 						perfect_filter_register_address+4+(mc_index * 8),
 						(u32)macaddr_high | NES_MAC_ADDR_VALID |
-						((((u32)(1<<mc_nic_index)) << 16)));
+					((((u32)(mc_nic_index)) << 16)));
 				nesadapter->pft_mcast_map[mc_index] =
 							nesvnic->nic_index;
 			} else {
@@ -1719,8 +1719,11 @@ struct net_device *nes_netdev_init(struct nes_device *nesdev,
 			(nesvnic->nesdev->nesadapter->port_count == 1 &&
 			nesvnic->nesdev->nesadapter->adapter_fcn_count == 2)) {
 				nesvnic->qp_nic_index[0] = nesvnic->nic_index;
-				nesvnic->qp_nic_index[1] = nesvnic->nic_index
-									+ 2;
+
+			if (nes_drv_opt & NES_DRV_OPT_MCAST_LOGPORT_MAP)
+				nesvnic->qp_nic_index[1] = 0xf;
+			else
+				nesvnic->qp_nic_index[1] = nesvnic->nic_index+2;
 				nesvnic->qp_nic_index[2] = 0xf;
 				nesvnic->qp_nic_index[3] = 0xf;
 		} else {
diff --git a/drivers/infiniband/hw/nes/nes_ud.c b/drivers/infiniband/hw/nes/nes_ud.c
new file mode 100644
index 0000000..33558e8
--- /dev/null
+++ b/drivers/infiniband/hw/nes/nes_ud.c
@@ -0,0 +1,2064 @@
+/*
+ * Copyright (c) 2008 - 2010 Intel Corporation.  All rights reserved.
+ * Copyright (c) 2006 - 2008 Neteffect, All rights reserved.
+ * Copyright (c) 2005 Open Grid Computing, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/version.h>
+#include <linux/vmalloc.h>
+#include <linux/completion.h>
+#include <linux/mutex.h>
+#include <linux/poll.h>
+#include <linux/idr.h>
+#include <linux/in.h>
+#include <linux/in6.h>
+#include <linux/device.h>
+#include <linux/netdevice.h>
+#include <linux/list.h>
+#include <linux/miscdevice.h>
+#include <linux/device.h>
+
+#include <rdma/ib_umem.h>
+#include <rdma/ib_user_verbs.h>
+
+#include "nes.h"
+#include "nes_ud.h"
+
+#define NES_UD_BASE_XMIT_NIC_QPID  28
+#define NES_UD_BASE_RECV_NIC_IDX   12
+#define NES_UD_BASE_XMIT_NIC_IDX   8
+#define NES_UD_MAX_NIC_CNT 8
+#define NES_UD_CLEANUP_TIMEOUT (HZ)
+#define NES_UD_MCAST_TBL_SZ 128
+#define NES_UD_SINGLE_HDR_SZ 64
+#define NES_UD_CQE_NUM NES_NIC_WQ_SIZE
+#define NES_UD_SKSQ_WAIT_TIMEOUT	100000
+#define NES_UD_MAX_REG_CNT 128
+
+#define NES_UD_MAX_ADAPTERS 4 /* number of supported interfaces for  RAW ETH */
+
+#define NES_UD_MAX_REG_HASH_CNT 256 /* last byte of the STAG is hash key */
+
+/*
+ * the same multicast could be allocated up to 2 owners so there could be
+ *  two differentmcast entries allocated for the same mcas address
+ */
+struct nes_ud_file;
+struct nes_ud_mcast {
+	u8 addr[3];
+	u8 in_use;
+	struct nes_ud_file *owner;
+	u8 nic_mask;
+};
+
+struct nes_ud_mem_region {
+	struct list_head list;
+	dma_addr_t *addrs;
+	u64 va;
+	u64 length;
+	u32 pg_cnt;
+	u32 in_use;
+	u32 stag; /* stag related this structure */
+};
+
+struct nic_queue_info {
+	u32 qpn;
+	u32 nic_index;
+	u32 logical_port;
+	enum nes_ud_dev_priority prio;
+	enum nes_ud_queue_type queue_type;
+	struct nes_ud_file *file;
+	struct nes_ud_file file_body;
+};
+
+struct nes_ud_resources {
+	int num_logport_confed;
+	int num_allocated_nics;
+	u8 logport_2_map;
+	u8 logport_3_map;
+	u32 original_6000;
+	u32 original_60b8;
+	struct nic_queue_info nics[NES_UD_MAX_NIC_CNT];
+	struct mutex          mutex;
+	struct nes_ud_mcast mcast[NES_UD_MCAST_TBL_SZ];
+	u32 adapter_no;  /* the allocated adapter no */
+
+	/* the unique ID of the NE020 adapter */
+	/*- it is allocated once per HW */
+	struct nes_adapter *pAdap;
+};
+
+/* memory hash list entry */
+struct nes_ud_hash_mem {
+    struct list_head   list;
+    int                read_stats;
+};
+
+
+
+struct nes_ud_mem {
+	/* hash list of registered STAGs */
+	struct nes_ud_hash_mem mrs[NES_UD_MAX_REG_HASH_CNT];
+	struct mutex          mutex;
+};
+
+/* the QP in format x.y.z where x is adapter no, */
+/* y is ud file idx in adapter, z is a qp no */
+static struct nes_ud_mem ud_mem;
+
+struct nes_ud_send_wr {
+    u32               wr_cnt;
+    u32               qpn;
+    u32               flags;
+    u32               resv[1];
+    struct ib_sge     sg_list[64];
+};
+
+struct nes_ud_recv_wr {
+    u32               wr_cnt;
+    u32               qpn;
+    u32	              resv[2];
+    struct ib_sge     sg_list[64];
+};
+
+static struct nes_ud_resources nes_ud_rsc[NES_UD_MAX_ADAPTERS];
+static struct workqueue_struct *nes_ud_workqueue;
+
+/*
+ * locate_ud_adapter
+ *
+ * the function locates the UD adapter
+*  on base of the adapter unique ID (structure nes_adapter)
+ */
+static inline
+struct nes_ud_resources *locate_ud_adapter(struct nes_adapter *pAdapt)
+{
+	int i;
+	struct nes_ud_resources *pRsc;
+
+	for (i = 0; i < NES_UD_MAX_ADAPTERS; i++) {
+		pRsc = &nes_ud_rsc[i];
+
+		if (pRsc->pAdap == pAdapt)
+			return pRsc;
+
+	}
+	return NULL;
+}
+
+/*
+ * allocate_ud_adapter()
+ *
+ * function allocates a new adapter
+ */
+static inline
+struct nes_ud_resources *allocate_ud_adapter(struct nes_adapter *pAdapt)
+{
+    int i;
+    struct nes_ud_resources *pRsc;
+
+    for (i = 0; i < NES_UD_MAX_ADAPTERS; i++) {
+	pRsc = &nes_ud_rsc[i];
+	if (pRsc->pAdap == NULL) {
+		pRsc->pAdap = pAdapt;
+		nes_debug(NES_DBG_UD, "new UD Adapter allocated %d"
+		" for adapter %p no =%d\n", i, pAdapt, pRsc->adapter_no);
+		return pRsc;
+		}
+	}
+	nes_debug(NES_DBG_UD, "Unable to allocate adapter\n");
+	return NULL;
+}
+
+static inline
+struct nes_ud_file *allocate_nic_queue(struct nes_vnic *nesvnic,
+					enum nes_ud_queue_type queue_type)
+{
+    struct nes_ud_file *file = NULL;
+	int i = 0;
+	u8 select_log_port = 0xf;
+	struct nes_device *nesdev = nesvnic->nesdev;
+	int log_port_2_alloced = 0;
+	int log_port_3_alloced = 0;
+	int ret = 0;
+	struct nes_ud_resources *pRsc;
+
+	/* the first thing that must be done is determine the adapter */
+	/* number max the adapter could have up to 2 interfaces */
+	if (nesvnic->nic_index != 0 && nesvnic->nic_index != 1) {
+		nes_debug(NES_DBG_UD, "nic queue allocation failed"
+		" nesvnic->nic_index = %d\n", nesvnic->nic_index);
+		return NULL;
+	}
+
+	/* locate device on base of nesvnic */
+	/* - when it is an unknown card a new one is allocated */
+	pRsc = locate_ud_adapter(nesdev->nesadapter);
+	if (pRsc == NULL)
+		return NULL;
+
+	for (i = 0; i < NES_UD_MAX_NIC_CNT; i++) {
+		if (pRsc->nics[i].file->active == 0)
+			continue;
+		if (pRsc->nics[i].logical_port == 2 &&
+		    queue_type == pRsc->nics[i].queue_type)
+				log_port_2_alloced++;
+		if (pRsc->nics[i].logical_port == 3 &&
+		    queue_type == pRsc->nics[i].queue_type)
+				log_port_3_alloced++;
+	}
+
+	/* check dual/single card */
+	if (pRsc->logport_2_map != pRsc->logport_3_map) {
+		/* a dual port card */
+		/* allocation is NIC2, NIC2, NIC3, NIC3 */
+		/*- no RX packat replication supported */
+		if (log_port_2_alloced < 2 &&
+		    pRsc->logport_2_map == nesvnic->nic_index)
+				select_log_port = 2;
+		else if (log_port_3_alloced < 2 &&
+			 pRsc->logport_3_map == nesvnic->nic_index)
+				select_log_port = 3;
+	} else {
+		/* single port card */
+		/* change allocation scheme to NIC2,NIC3,NIC2,NIC3 */
+		switch (log_port_2_alloced + log_port_3_alloced) {
+		case 0: /* no QPs allocated - use NIC2 */
+			if (pRsc->logport_2_map == nesvnic->nic_index)
+				select_log_port = 2;
+
+			break;
+		case 1: /* NIC2 or NIC3 allocated */
+			if (log_port_2_alloced > 0) {
+				/* if NIC2 allocated use NIC3 */
+				if (pRsc->logport_3_map == nesvnic->nic_index)
+					select_log_port = 3;
+
+			} else {
+				/* when NIC3 allocated use NIC2 */
+				if (pRsc->logport_2_map == nesvnic->nic_index)
+					select_log_port = 2;
+
+			}
+			break;
+
+		case 2:
+		/* NIC2 and NIC3 allocated or both ports on NIC3 - use NIC2 */
+			if ((log_port_2_alloced == 1) ||
+			    (log_port_3_alloced == 2)) {
+				if (pRsc->logport_2_map == nesvnic->nic_index)
+					select_log_port = 2;
+
+			} else {
+				/* both ports allocated on NIC2 - use NIC3 */
+				if (pRsc->logport_3_map == nesvnic->nic_index)
+					select_log_port = 3;
+
+			}
+				break;
+		case 3:
+			/* when both NIC2 allocated use NIC3 */
+			if (log_port_2_alloced == 2) {
+				if (pRsc->logport_3_map == nesvnic->nic_index)
+					select_log_port = 3;
+
+			} else {
+				/* when both NIC3 alloced use NIC2 */
+				if (pRsc->logport_2_map == nesvnic->nic_index)
+					select_log_port = 2;
+			}
+			break;
+
+		default:
+			break;
+		}
+	}
+	if (select_log_port == 0xf) {
+		ret = -1;
+		nes_debug(NES_DBG_UD, "%s(%d) logport allocation failed "
+		"log_port_2_alloced=%d log_port_3_alloced=%d\n",
+			__func__, __LINE__, log_port_2_alloced,
+			log_port_3_alloced);
+		goto out;
+	}
+
+	nes_debug(NES_DBG_UD, "%s(%d) log_port_2_alloced=%d "
+		"log_port_3_alloced=%d select_log_port=%d\n",
+		__func__, __LINE__, log_port_2_alloced,
+		log_port_3_alloced, select_log_port);
+
+	for (i = 0; i < NES_UD_MAX_NIC_CNT; i++) {
+		if (pRsc->nics[i].file->active == 1)
+			continue;
+		if (pRsc->nics[i].logical_port == select_log_port &&
+		    queue_type == pRsc->nics[i].queue_type) {
+
+			/* file is preallocated during initialization */
+			file = pRsc->nics[i].file;
+			memset(file, 0, sizeof(*file));
+
+			file->nesvnic = nesvnic;
+			file->queue_type = queue_type;
+
+			file->prio = pRsc->nics[i].prio;
+			file->qpn = pRsc->nics[i].qpn;
+			file->nes_ud_nic_index = pRsc->nics[i].nic_index;
+			file->rsc_idx = i;
+			file->adapter_no = pRsc->adapter_no;
+			goto out;
+		}
+	}
+
+out:
+	return file;
+}
+
+static inline int  del_rsc_list(struct nes_ud_file *file)
+{
+	int logport_2_cnt = 0;
+	int logport_3_cnt = 0;
+	struct nes_device *nesdev = file->nesvnic->nesdev;
+	int i = 0;
+	struct nes_ud_resources *pRsc;
+
+	if (file == NULL) {
+		nes_debug(NES_DBG_UD, "%s(%d) file is NULL\n",
+			__func__, __LINE__);
+		return -EFAULT;
+	}
+	if (file->nesvnic == NULL) {
+		nes_debug(NES_DBG_UD, "%s(%d) file->nesvnic is NULL\n",
+			__func__, __LINE__);
+		return -EFAULT;
+	}
+	if (nesdev == NULL) {
+		nes_debug(NES_DBG_UD, "%s(%d) nesdev is NULL\n",
+			__func__, __LINE__);
+		return -EFAULT;
+	}
+
+	/* locate device on base of nesvnic */
+	/*- when it is an unknown card a new one is allocated */
+	pRsc = locate_ud_adapter(nesdev->nesadapter);
+	if (pRsc == NULL) {
+		nes_debug(NES_DBG_UD, "%s(%d) cannot locate an allocated "
+			"adapter  is NULL\n", __func__, __LINE__);
+		return -EFAULT;
+	}
+	if (--pRsc->num_allocated_nics == 0) {
+		nes_write_indexed(nesdev, 0x60b8, pRsc->original_60b8);
+		nes_write_indexed(nesdev, 0x6000, pRsc->original_6000);
+		pRsc->num_logport_confed = 0;
+	}
+	BUG_ON(pRsc->num_allocated_nics < 0);
+	BUG_ON(file->rsc_idx >= NES_UD_MAX_NIC_CNT);
+
+	for (i = 0; i < NES_UD_MAX_NIC_CNT; i++) {
+		if (pRsc->nics[i].file->active &&
+		    pRsc->nics[i].logical_port == 2)
+			logport_2_cnt++;
+		if (pRsc->nics[i].file->active &&
+		    pRsc->nics[i].logical_port == 3)
+			logport_3_cnt++;
+	}
+
+	if (pRsc->num_logport_confed != 0x3 && logport_2_cnt == 0)
+		pRsc->logport_2_map = 0xf;
+
+	if (pRsc->num_logport_confed != 0x3 && logport_3_cnt == 0)
+		pRsc->logport_3_map = 0xf;
+	return 0;
+}
+
+/*
+* the QPN contains now the number of the RAW ETH
+* adapter and QPN number on the adapter
+* the adapter number is located in the highier
+* 8 bits so QPN is stored as [adapter:qpn]
+*/
+static inline
+struct nes_ud_file *get_file_by_qpn(struct nes_ud_resources *pRsc, int qpn)
+{
+	int i = 0;
+
+	for (i = 0; i < NES_UD_MAX_NIC_CNT; i++) {
+		if (pRsc->nics[i].file->active &&
+		    pRsc->nics[i].qpn == (qpn & 0xff))
+			return pRsc->nics[i].file;
+
+	}
+	return NULL;
+}
+
+/* function counts all ETH RAW entities that have  */
+/* a specific type and relation to specific vnic */
+static inline
+int count_files_by_nic(struct nes_vnic *nesvnic,
+			enum nes_ud_queue_type queue_type)
+{
+	int count = 0;
+	int i = 0;
+	struct nes_ud_resources *pRsc;
+
+	pRsc = locate_ud_adapter(nesvnic->nesdev->nesadapter);
+	if (pRsc == NULL)
+		return 0;
+
+	for (i = 0; i < NES_UD_MAX_NIC_CNT; i++) {
+		if (pRsc->nics[i].file->active &&
+		    pRsc->nics[i].file->nesvnic == nesvnic &&
+		    pRsc->nics[i].queue_type == queue_type)
+				count++;
+	}
+	return count;
+}
+
+/* function counts all RAW ETH  entities the have a specific type */
+static inline
+int count_files(struct nes_vnic *nesvnic, enum nes_ud_queue_type queue_type)
+{
+	int count = 0;
+	int i = 0;
+	struct nes_ud_resources *pRsc;
+
+	pRsc = locate_ud_adapter(nesvnic->nesdev->nesadapter);
+	if (pRsc == NULL)
+		return 0;
+
+	for (i = 0; i < NES_UD_MAX_NIC_CNT; i++) {
+		if (pRsc->nics[i].file->active &&
+		    pRsc->nics[i].queue_type == queue_type)
+			count++;
+	}
+	return count;
+}
+
+/*
+ * the function locates the entry allocated by IGMP and modifies the
+ * PFT entry with the list of the NICs allowed to receive that multicast
+ * the NIC0/NIC1 are removed due to performance issue so tcpdum
+ * like tools cannot receive the accelerated multicasts
+ */
+static void mcast_fix_filter_table_single(struct nes_ud_file *file, u8 *addr)
+{
+  struct nes_device *nesdev = file->nesvnic->nesdev;
+  int i = 0;
+  u32 macaddr_low;
+  u32 orig_low;
+  u32 macaddr_high;
+  u32 prev_high;
+
+  for (i = 0; i < 48; i++) {
+	macaddr_low = nes_read_indexed(nesdev,
+					NES_IDX_PERFECT_FILTER_LOW + i*8);
+	orig_low = macaddr_low;
+	macaddr_high = nes_read_indexed(nesdev,
+					NES_IDX_PERFECT_FILTER_LOW + 4 + i*8);
+	if (!(macaddr_high & NES_MAC_ADDR_VALID))
+		continue;
+	if ((macaddr_high & 0xffff) != 0x0100)
+		continue;
+	if ((macaddr_low & 0xff) != addr[2])
+		continue;
+	macaddr_low >>= 8;
+	if ((macaddr_low & 0xff) != addr[1])
+		continue;
+	macaddr_low >>= 8;
+	if ((macaddr_low & 0xff) != addr[0])
+		continue;
+	macaddr_low >>= 8;
+	if ((macaddr_low & 0xff) != 0x5e)
+		continue;
+	/* hit - that means Linux or other UD set this bit earlier  */
+	prev_high = macaddr_high;
+	nes_write_indexed(nesdev, NES_IDX_PERFECT_FILTER_LOW + 4 + i*8, 0);
+	macaddr_high = (macaddr_high & 0xfffcffff) |
+					((1<<file->nes_ud_nic_index) << 16);
+
+	nes_debug(NES_DBG_UD, "%s(%d) found addr to fix, "
+		 "i=%d, macaddr_high=0x%x  macaddr_low=0x%x "
+		 "nic_idx=%d prev_high=0x%x\n",
+		 __func__, __LINE__, i, macaddr_high, orig_low,
+		file->nes_ud_nic_index, prev_high);
+	nes_write_indexed(nesdev,
+			NES_IDX_PERFECT_FILTER_LOW + 4 + i*8, macaddr_high);
+	break;
+  }
+}
+
+/* this function is implemented that way because the Linux multicast API
+   use the multicast list approach. When a new multicast address is added
+   all PFT table is reinitialized by linux and all entries must be fixed
+   by this procedure
+*/
+static void mcast_fix_filter_table(struct nes_ud_file *file)
+{
+	int i;
+	struct nes_ud_resources *pRsc;
+
+	pRsc = locate_ud_adapter(file->nesvnic->nesdev->nesadapter);
+	if (pRsc == NULL)
+		return;
+
+	for (i = 0; i < NES_UD_MCAST_TBL_SZ; i++) {
+		if (pRsc->mcast[i].in_use != 0)
+			mcast_fix_filter_table_single(pRsc->mcast[i].owner,
+							pRsc->mcast[i].addr);
+	}
+}
+
+/* function invalidates the PFT entry */
+static void remove_mcast_from_pft(struct nes_ud_file *file, u8 *addr)
+{
+  struct nes_device *nesdev = file->nesvnic->nesdev;
+  int i = 0;
+   u32 macaddr_low;
+  u32 orig_low;
+  u32 macaddr_high;
+  u32 prev_high;
+
+  for (i = 0; i < 48; i++) {
+	macaddr_low = nes_read_indexed(nesdev,
+					NES_IDX_PERFECT_FILTER_LOW + i*8);
+	orig_low = macaddr_low;
+	macaddr_high = nes_read_indexed(nesdev,
+					NES_IDX_PERFECT_FILTER_LOW + 4 + i*8);
+	if (!(macaddr_high & NES_MAC_ADDR_VALID))
+		continue;
+
+	if ((macaddr_high & 0xffff) != 0x0100)
+		continue;
+	if ((macaddr_low & 0xff) != addr[2])
+		continue;
+	macaddr_low >>= 8;
+	if ((macaddr_low & 0xff) != addr[1])
+		continue;
+	macaddr_low >>= 8;
+	if ((macaddr_low & 0xff) != addr[0])
+		continue;
+	macaddr_low >>= 8;
+	if ((macaddr_low & 0xff) != 0x5e)
+		continue;
+	/* hit - that means Linux or other UD set this bit earlier */
+	/* so remove the NIC from MAC address reception */
+	prev_high = macaddr_high;
+	macaddr_high = (macaddr_high & 0xfffcffff) &
+					~((1<<file->nes_ud_nic_index) << 16);
+	nes_debug(NES_DBG_UD, "%s(%d) found addr to mcast remove,"
+		"i=%d, macaddr_high=0x%x  macaddr_low=0x%x "
+		"nic_idx=%d prev_high=0x%x\n", __func__, __LINE__, i,
+		macaddr_high, orig_low, file->nes_ud_nic_index, prev_high);
+	nes_write_indexed(nesdev, NES_IDX_PERFECT_FILTER_LOW + 4 + i*8,
+							macaddr_high);
+	break;
+	}
+
+}
+
+/*
+* the function returns a mask of the NICs
+* assotiated with given multicast address
+*/
+static int nes_ud_mcast_filter(struct nes_vnic *nesvnic, __u8 *dmi_addr)
+{
+	int i = 0;
+	int ret = 0;
+	int mask = 0;
+	struct nes_ud_resources *pRsc;
+
+	pRsc = locate_ud_adapter(nesvnic->nesdev->nesadapter);
+	if (pRsc == NULL)
+		return 0;
+
+	for (i = 0; i < NES_UD_MCAST_TBL_SZ; i++) {
+		if (pRsc->mcast[i].in_use &&
+		    pRsc->mcast[i].addr[0] == dmi_addr[3] &&
+		    pRsc->mcast[i].addr[1] == dmi_addr[4] &&
+		    pRsc->mcast[i].addr[2] == dmi_addr[5]) {
+			mask = (pRsc->mcast[i].owner->mcast_mode ==
+				NES_UD_MCAST_PFT_MODE) ?
+				pRsc->mcast[i].owner->nes_ud_nic_index : 0;
+
+			ret = ret | (1 << mask);
+			nes_debug(NES_DBG_UD, "mcast filter, "
+				"fpr=%02X%02X%02X ret=%d\n",
+				dmi_addr[3], dmi_addr[4], dmi_addr[5], ret);
+		}
+	}
+	if (ret == 0)
+		return -1;
+	else
+		return ret;
+
+}
+
+static __u32 mqueue_key[4] = { 0x0, 0x80, 0x0, 0x0 };
+
+static inline __u8 nes_ud_calculate_hash(__u8 dest_addr_lsb)
+{
+  __u8 in[8];
+  __u32 key_arr[4];
+  int i;
+  __u32 result = 0;
+  int j, k;
+  __u8 shift_in, next_shift_in;
+
+  in[0] = 0;
+  in[1] = 0;
+  in[2] = 0;
+  in[3] = 0;
+
+  in[4] = 0;
+
+  in[5] = 0;
+  in[6] = 0;
+  in[7] = dest_addr_lsb;
+
+
+
+	for (i = 0; i < 4; i++)
+		key_arr[3-i] = mqueue_key[i];
+
+
+
+	for (i = 0; i < 8; i++) {
+		for (j = 7; j >= 0; j--) {
+			if (in[i] & (1 << j))
+				result = result ^ key_arr[0];
+
+			shift_in = 0;
+			for (k = 3; k >= 0; k--) {
+				next_shift_in = key_arr[k] >> 31;
+				key_arr[k] = (key_arr[k] << 1) + shift_in;
+				shift_in = next_shift_in;
+			}
+		}
+	}
+	return result & 0x7f;
+}
+
+static inline void nes_ud_enable_mqueue(struct nes_ud_file *file)
+{
+	struct nes_device *nesdev = file->nesvnic->nesdev;
+	int mqueue_config0;
+	int mqueue_config2;
+	int instance = file->nes_ud_nic_index & 0x1;
+
+	mqueue_config0 = nes_read_indexed(nesdev, 0x6400);
+	mqueue_config0 |= (4 | (instance & 0x3)) << (file->nes_ud_nic_index*3);
+	nes_write_indexed(nesdev, 0x6400, mqueue_config0);
+	mqueue_config0 = nes_read_indexed(nesdev, 0x6400);
+
+	mqueue_config2 = nes_read_indexed(nesdev, 0x6408);
+	mqueue_config2 |= (2 << (instance*2)) | (6 << (instance*3+8));
+	nes_write_indexed(nesdev, 0x6408, mqueue_config2);
+	mqueue_config2 = nes_read_indexed(nesdev, 0x6408);
+
+	nes_write_indexed(nesdev, 0x64a0+instance*0x100, mqueue_key[0]);
+	nes_write_indexed(nesdev, 0x64a4+instance*0x100, mqueue_key[1]);
+	nes_write_indexed(nesdev, 0x64a8+instance*0x100, mqueue_key[2]);
+	nes_write_indexed(nesdev, 0x64ac+instance*0x100, mqueue_key[3]);
+
+	nes_debug(NES_DBG_UD, "mq_config0=0x%x mq_config2=0x%x nic_idx= %d\n",
+		  mqueue_config0, mqueue_config2, file->nes_ud_nic_index);
+
+}
+
+
+
+static inline
+void nes_ud_redirect_from_mqueue(struct nes_ud_file *file, int num_queues)
+{
+	struct nes_device *nesdev = file->nesvnic->nesdev;
+	int instance = file->nes_ud_nic_index & 0x1;
+	unsigned addr = 0x6420+instance*0x100;
+	unsigned value;
+	int i;
+
+	value  = (file->prio == NES_UD_DEV_PRIO_LOW || num_queues == 1) ?
+							0x0 : 0x11111111;
+	for (i = 0; i < 16; i++)
+		nes_write_indexed(nesdev, addr+i*4, value);
+}
+
+
+static int nes_ud_create_nic(struct nes_ud_file *file)
+{
+	struct nes_vnic *nesvnic = file->nesvnic;
+	struct nes_device *nesdev = nesvnic->nesdev;
+	struct nes_hw_nic_qp_context *nic_context;
+	struct nes_hw_cqp_wqe *cqp_wqe;
+	struct nes_cqp_request *cqp_request;
+	unsigned long flags;
+	void *vmem;
+	dma_addr_t pmem;
+	u64 u64temp;
+	int ret = 0;
+
+	BUG_ON(file->nic_vbase != NULL);
+
+	file->nic_mem_size = 256 +
+			(NES_NIC_WQ_SIZE * sizeof(struct nes_hw_nic_sq_wqe)) +
+			sizeof(struct nes_hw_nic_qp_context);
+
+	file->nic_vbase = pci_alloc_consistent(nesdev->pcidev,
+						file->nic_mem_size,
+						&file->nic_pbase);
+	if (!file->nic_vbase) {
+		nes_debug(NES_DBG_UD, "Unable to allocate memory for NIC host "
+			"descriptor rings\n");
+		return -ENOMEM;
+	}
+
+	memset(file->nic_vbase, 0, file->nic_mem_size);
+
+	vmem = (void *)(((unsigned long long)file->nic_vbase + (256 - 1)) &
+			~(unsigned long long)(256 - 1));
+	pmem = (dma_addr_t)(((unsigned long long)file->nic_pbase + (256 - 1)) &
+			~(unsigned long long)(256 - 1));
+
+	file->wq_vbase = vmem;
+	file->wq_pbase = pmem;
+	file->head = 0;
+	file->tail = 0;
+
+	vmem += (NES_NIC_WQ_SIZE * sizeof(struct nes_hw_nic_sq_wqe));
+	pmem += (NES_NIC_WQ_SIZE * sizeof(struct nes_hw_nic_sq_wqe));
+
+	cqp_request = nesvnic->get_cqp_request(nesdev);
+	if (cqp_request == NULL) {
+		nes_debug(NES_DBG_QP, "Failed to get a cqp_request.\n");
+		goto fail_cqp_req_alloc;
+	}
+	cqp_request->waiting = 1;
+	cqp_wqe = &cqp_request->cqp_wqe;
+
+	cqp_wqe->wqe_words[NES_CQP_WQE_OPCODE_IDX] =
+			cpu_to_le32(NES_CQP_CREATE_QP | NES_CQP_QP_TYPE_NIC);
+	cqp_wqe->wqe_words[NES_CQP_WQE_ID_IDX] = cpu_to_le32(file->qpn);
+	cqp_wqe->wqe_words[NES_CQP_WQE_COMP_CTX_LOW_IDX] =
+			cpu_to_le32((u32)((u64)(&nesdev->cqp)));
+	cqp_wqe->wqe_words[NES_CQP_WQE_COMP_CTX_HIGH_IDX] =
+			cpu_to_le32((u32)(((u64)(&nesdev->cqp))>>32));
+	cqp_wqe->wqe_words[NES_CQP_WQE_COMP_SCRATCH_LOW_IDX] = 0;
+	cqp_wqe->wqe_words[NES_CQP_WQE_COMP_SCRATCH_HIGH_IDX] = 0;
+
+
+	nic_context = vmem;
+
+	nic_context->context_words[NES_NIC_CTX_MISC_IDX] =
+			cpu_to_le32((u32)NES_NIC_CTX_SIZE |
+			((u32)PCI_FUNC(nesdev->pcidev->devfn) << 12) |
+			(1 << 18));
+
+	nic_context->context_words[NES_NIC_CTX_SQ_LOW_IDX] = 0;
+	nic_context->context_words[NES_NIC_CTX_SQ_HIGH_IDX] = 0;
+	nic_context->context_words[NES_NIC_CTX_RQ_LOW_IDX] = 0;
+	nic_context->context_words[NES_NIC_CTX_RQ_HIGH_IDX] = 0;
+
+	u64temp = (u64)file->wq_pbase;
+	if (file->queue_type == NES_UD_SEND_QUEUE) {
+		nic_context->context_words[NES_NIC_CTX_SQ_LOW_IDX] =
+						cpu_to_le32((u32)u64temp);
+		nic_context->context_words[NES_NIC_CTX_SQ_HIGH_IDX] =
+					cpu_to_le32((u32)(u64temp >> 32));
+	} else {
+		nic_context->context_words[NES_NIC_CTX_RQ_LOW_IDX] =
+						cpu_to_le32((u32)u64temp);
+		nic_context->context_words[NES_NIC_CTX_RQ_HIGH_IDX] =
+					cpu_to_le32((u32)(u64temp >> 32));
+	}
+
+	u64temp = (u64)pmem;
+
+	cqp_wqe->wqe_words[NES_CQP_QP_WQE_CONTEXT_LOW_IDX] =
+						cpu_to_le32((u32)u64temp);
+	cqp_wqe->wqe_words[NES_CQP_QP_WQE_CONTEXT_HIGH_IDX] =
+					cpu_to_le32((u32)(u64temp >> 32));
+
+	atomic_set(&cqp_request->refcount, 2);
+	nesvnic->post_cqp_request(nesdev, cqp_request);
+
+	/* Wait for CQP */
+	wait_event_timeout(cqp_request->waitq,
+				(cqp_request->request_done != 0),
+				NES_EVENT_TIMEOUT);
+
+	if (atomic_dec_and_test(&cqp_request->refcount)) {
+		if (cqp_request->dynamic) {
+			kfree(cqp_request);
+		} else {
+			spin_lock_irqsave(&nesdev->cqp.lock, flags);
+			list_add_tail(&cqp_request->list,
+						&nesdev->cqp_avail_reqs);
+			spin_unlock_irqrestore(&nesdev->cqp.lock, flags);
+		}
+	}
+	nes_debug(NES_DBG_UD, "Created NIC, qpn=%d, SQ/RQ pa=0x%p va=%p "
+		"virt_to_phys=%p\n", file->qpn,
+		(void *)file->wq_pbase, (void *)file->nic_vbase,
+		(void *)virt_to_phys(file->nic_vbase));
+	return ret;
+
+ fail_cqp_req_alloc:
+	pci_free_consistent(nesdev->pcidev, file->nic_mem_size, file->nic_vbase,
+			file->nic_pbase);
+	file->nic_vbase = NULL;
+	return -EFAULT;
+}
+
+
+static void nes_ud_destroy_nic(struct nes_ud_file *file)
+{
+	struct nes_vnic *nesvnic = file->nesvnic;
+	struct nes_device *nesdev = nesvnic->nesdev;
+	struct nes_hw_cqp_wqe *cqp_wqe;
+	struct nes_cqp_request *cqp_request;
+	unsigned long flags;
+	int ret = 0;
+
+	cqp_request = nesvnic->get_cqp_request(nesdev);
+	if (cqp_request == NULL) {
+		nes_debug(NES_DBG_QP, "Failed to get a cqp_request.\n");
+		return;
+	}
+	cqp_request->waiting = 1;
+	cqp_wqe = &cqp_request->cqp_wqe;
+
+	cqp_wqe->wqe_words[NES_CQP_WQE_OPCODE_IDX] =
+			cpu_to_le32(NES_CQP_DESTROY_QP | NES_CQP_QP_TYPE_NIC);
+	cqp_wqe->wqe_words[NES_CQP_WQE_ID_IDX] = cpu_to_le32(file->qpn);
+	cqp_wqe->wqe_words[NES_CQP_WQE_COMP_CTX_LOW_IDX] =
+			cpu_to_le32((u32)((u64)(&nesdev->cqp)));
+	cqp_wqe->wqe_words[NES_CQP_WQE_COMP_CTX_HIGH_IDX] =
+			cpu_to_le32((u32)(((u64)(&nesdev->cqp)) >> 32));
+	cqp_wqe->wqe_words[NES_CQP_WQE_COMP_SCRATCH_LOW_IDX] = 0;
+	cqp_wqe->wqe_words[NES_CQP_WQE_COMP_SCRATCH_HIGH_IDX] = 0;
+
+	atomic_set(&cqp_request->refcount, 2);
+	nesvnic->post_cqp_request(nesdev, cqp_request);
+
+	/* Wait for CQP */
+	ret = wait_event_timeout(cqp_request->waitq,
+			(cqp_request->request_done != 0),
+			NES_EVENT_TIMEOUT);
+	if (!ret)
+		nes_debug(NES_DBG_UD, "NES_UD NIC QP%u "
+			"destroy timeout expired\n", file->qpn);
+
+	if (atomic_dec_and_test(&cqp_request->refcount)) {
+		if (cqp_request->dynamic) {
+			kfree(cqp_request);
+		} else {
+			spin_lock_irqsave(&nesdev->cqp.lock, flags);
+			list_add_tail(&cqp_request->list,
+					&nesdev->cqp_avail_reqs);
+			spin_unlock_irqrestore(&nesdev->cqp.lock, flags);
+		}
+	}
+
+	pci_free_consistent(nesdev->pcidev, file->nic_mem_size, file->nic_vbase,
+			file->nic_pbase);
+	file->nic_vbase = NULL;
+	file->qp_ptr = NULL;
+
+	return;
+}
+
+static void nes_ud_free_resources(struct nes_ud_file *file)
+{
+	struct nes_device *nesdev = file->nesvnic->nesdev;
+	int nic_active = 0;
+	int mcast_all = 0;
+	int mcast_en = 0;
+	int wqm_config0 = 0;
+	wait_queue_head_t     waitq;
+	int num_queues  = 0;
+	nes_debug(NES_DBG_UD, " %s(%d) NAME=%s nes_ud_qpid=%d\n",
+		__func__, __LINE__, file->ifrn_name, file->qpn);
+
+	if (!file->nesvnic || !file->active)
+		return;
+
+	if (file->queue_type == NES_UD_SEND_QUEUE) {
+		nic_active = nes_read_indexed(nesdev, NES_IDX_NIC_ACTIVE);
+		nic_active &= ~(1 << file->nes_ud_nic_index);
+		nes_write_indexed(nesdev, NES_IDX_NIC_ACTIVE, nic_active);
+		nic_active = nes_read_indexed(nesdev, NES_IDX_NIC_ACTIVE);
+	} else {
+		num_queues = count_files_by_nic(file->nesvnic,
+						file->queue_type);
+
+	if (num_queues == 1) {
+
+		nic_active = nes_read_indexed(nesdev, NES_IDX_NIC_ACTIVE);
+		nic_active &= ~(1 << file->nes_ud_nic_index);
+		nes_write_indexed(nesdev, NES_IDX_NIC_ACTIVE, nic_active);
+		nic_active = nes_read_indexed(nesdev, NES_IDX_NIC_ACTIVE);
+
+		mcast_all = nes_read_indexed(nesdev, NES_IDX_NIC_MULTICAST_ALL);
+		mcast_all &= ~(1 << file->nes_ud_nic_index);
+		nes_write_indexed(nesdev, NES_IDX_NIC_MULTICAST_ALL, mcast_all);
+		mcast_all = nes_read_indexed(nesdev, NES_IDX_NIC_MULTICAST_ALL);
+
+		mcast_en = nes_read_indexed(nesdev,
+						NES_IDX_NIC_MULTICAST_ENABLE);
+		mcast_en &= ~(1 << file->nes_ud_nic_index);
+		nes_write_indexed(nesdev, NES_IDX_NIC_MULTICAST_ENABLE,
+								mcast_en);
+		mcast_en = nes_read_indexed(nesdev,
+						NES_IDX_NIC_MULTICAST_ENABLE);
+
+		nes_debug(NES_DBG_UD, "nic_active=0x%x, mcast_en=0x%x, "
+			"mcast_all=0x%x nic_index=%d num_queues=%d\n",
+			nic_active, mcast_en, mcast_all,
+			file->nes_ud_nic_index, num_queues);
+	  }
+
+	 nes_ud_redirect_from_mqueue(file, num_queues);
+	 num_queues = count_files(file->nesvnic, file->queue_type);
+	if (num_queues == 1) {
+		nes_debug(NES_DBG_UD, "Last receive queue, "
+			"restoring MPP debug register\n");
+		nes_write_indexed(nesdev, 0xA00, 0x200);
+		nes_write_indexed(nesdev, 0xA40, 0x200);
+	  }
+	}
+
+
+
+	nes_ud_destroy_nic(file);
+
+	if (file->queue_type == NES_UD_RECV_QUEUE) {
+		wqm_config0 = nes_read_indexed(nesdev, 0x5000);
+		wqm_config0 &= ~0x8000;
+		nes_write_indexed(nesdev, 0x5000, wqm_config0);
+
+		init_waitqueue_head(&waitq);
+
+		wait_event_timeout(waitq, 0, NES_UD_CLEANUP_TIMEOUT);
+
+		nes_debug(NES_DBG_UD, "%s(%d) enabling stall_no_wqes\n",
+			__func__, __LINE__);
+		wqm_config0 = nes_read_indexed(nesdev, 0x5000);
+		wqm_config0 |= 0x8000;
+		nes_write_indexed(nesdev, 0x5000, wqm_config0);
+	}
+
+	dev_put(file->nesvnic->netdev);
+
+	file->active = 0;
+
+	nes_debug(NES_DBG_UD, "%s(%d) done\n", __func__, __LINE__);
+}
+
+
+static int nes_ud_init_channel(struct nes_ud_file *file)
+{
+	struct nes_device *nesdev = NULL;
+	int ret = 0;
+	int nic_active = 0;
+	int mcast_all = 0;
+	int mcast_en = 0;
+	int link_ag = 0;
+	int mpp4_dbg = 0;
+
+	nesdev = file->nesvnic->nesdev;
+
+	ret = nes_ud_create_nic(file);
+	if (ret != 0)
+		return ret;
+
+	if (file->queue_type == NES_UD_RECV_QUEUE) {
+
+		file->nesvnic->mcrq_mcast_filter = nes_ud_mcast_filter;
+
+		mcast_en = nes_read_indexed(nesdev,
+					NES_IDX_NIC_MULTICAST_ENABLE);
+		mcast_en |= 1 << file->nes_ud_nic_index;
+		nes_write_indexed(nesdev, NES_IDX_NIC_MULTICAST_ENABLE,
+								mcast_en);
+		mcast_en = nes_read_indexed(nesdev,
+					NES_IDX_NIC_MULTICAST_ENABLE);
+
+		/* the only case when we use PFT is for single port
+		two functions, which probably would be the
+		most common usage model :), but anyway */
+	if (file->mcast_mode == NES_UD_MCAST_ALL_MODE) {
+		mcast_all = nes_read_indexed(nesdev, NES_IDX_NIC_MULTICAST_ALL);
+		mcast_all |= 1 << file->nes_ud_nic_index;
+		nes_write_indexed(nesdev, NES_IDX_NIC_MULTICAST_ALL, mcast_all);
+		mcast_all = nes_read_indexed(nesdev, NES_IDX_NIC_MULTICAST_ALL);
+	}
+	if (nesdev->nesadapter->port_count <= 2) {
+		link_ag = 0x00;
+		nes_write_indexed(nesdev, 0x6038, link_ag);
+		link_ag = nes_read_indexed(nesdev, 0x6038);
+	}
+	if (nesdev->nesadapter->netdev_count <= 2)
+		nes_ud_enable_mqueue(file);
+
+	nes_write_indexed(nesdev, 0xA00, 0x245);
+	nes_write_indexed(nesdev, 0xA40, 0x245);
+
+	}
+	/* NES_UD_SEND_QUEUE */
+	else {
+		mpp4_dbg = nes_read_indexed(nesdev, 0xb00);
+		mpp4_dbg |= 1 << 12;
+		nes_write_indexed(nesdev, 0xb00, mpp4_dbg);
+		mpp4_dbg = nes_read_indexed(nesdev, 0xb00);
+	}
+
+	nic_active = nes_read_indexed(nesdev, NES_IDX_NIC_ACTIVE);
+	nic_active |= 1 << file->nes_ud_nic_index;
+	nes_write_indexed(nesdev, NES_IDX_NIC_ACTIVE, nic_active);
+	nic_active = nes_read_indexed(nesdev, NES_IDX_NIC_ACTIVE);
+
+	nes_debug(NES_DBG_UD, "nic_active=0x%x, mcast_en=0x%x, "
+		"mcast_all=0x%x nic_index=%d link_ag=0x%x mpp4_dbg=0x%x\n",
+		nic_active, mcast_en, mcast_all, file->nes_ud_nic_index,
+		link_ag, mpp4_dbg);
+
+	return ret;
+}
+
+static struct nes_ud_file *nes_ud_get_nxt_channel(struct nes_vnic *nesvnic,
+					enum nes_ud_queue_type queue_type)
+{
+	struct nes_ud_file *file = NULL;
+	struct net_device *netdev = NULL;
+	struct nes_device *nesdev = NULL;
+	struct nes_ud_resources *pRsc;
+
+	netdev = nesvnic->netdev;
+	nesdev = nesvnic->nesdev;
+
+	pRsc = locate_ud_adapter(nesdev->nesadapter);
+	if (pRsc == NULL) {
+		pRsc = allocate_ud_adapter(nesdev->nesadapter);
+		if (pRsc == NULL)
+			return NULL;
+
+	}
+	if (pRsc->num_logport_confed == 0) {
+		pRsc->original_60b8 = nes_read_indexed(nesdev, 0x60b8);
+		pRsc->original_6000 = nes_read_indexed(nesdev, 0x6000);
+		/* everything goes to port 0x0 */
+		if ((nesvnic->nesdev->nesadapter->port_count == 1) ||
+		    (nes_drv_opt & NES_DRV_OPT_MCAST_LOGPORT_MAP)) {
+			/* single port card or dual port using single if */
+			pRsc->num_logport_confed = 0x3;
+			pRsc->logport_2_map = 0x0;
+			pRsc->logport_3_map = 0x0;
+			nes_write_indexed(nesdev, 0x60b8, 0x3);
+			nes_write_indexed(nesdev, 0x6000, 0x0);
+		} else {
+			pRsc->num_logport_confed = 0x3;
+			pRsc->logport_2_map = 0x0;
+			pRsc->logport_3_map = 0x1;
+		}
+		nes_debug(NES_DBG_UD, "%s(%d) num_logport_confed=%d "
+			"original_6000=%d logport_3_map = %d nes_drv_opt=%x\n",
+			__func__, __LINE__, pRsc->num_logport_confed,
+			pRsc->original_6000, pRsc->logport_3_map, nes_drv_opt);
+	}
+
+	nes_debug(NES_DBG_UD, "%s(%d) logport_2_map=%d logport_3_map=%d\n",
+		 __func__, __LINE__, pRsc->logport_2_map, pRsc->logport_3_map);
+
+	file = allocate_nic_queue(nesvnic, queue_type);
+	if (file == NULL) {
+		nes_debug(NES_DBG_UD, "%s(%d) failed to allocate NIC\n",
+			__func__, __LINE__);
+		return NULL;
+	}
+
+	file->active = 1;
+	memcpy(file->ifrn_name, netdev->name, IFNAMSIZ);
+
+	/* for now use pft always */
+	file->mcast_mode = NES_UD_MCAST_PFT_MODE;
+
+	nes_debug(NES_DBG_UD, " %s(%d) NAME=%s qpn=%d nes_ud_nic_index=%d "
+		"nes_ud_nic.qp_id=%d mcast_mode=%d port_count=%d "
+		"netdev_count=%d\n", __func__, __LINE__, file->ifrn_name,
+		 file->qpn, file->nes_ud_nic_index, file->nesvnic->mcrq_qp_id,
+		 file->mcast_mode, nesdev->nesadapter->port_count,
+		nesdev->nesadapter->netdev_count);
+
+	file->mss = netdev->mtu-28;
+	pRsc->num_allocated_nics++;
+	BUG_ON(pRsc->num_allocated_nics > 8);
+
+	return file;
+
+}
+
+static struct nes_ud_mem_region *nes_ud_allocate_mr(u32 npages)
+{
+	struct nes_ud_mem_region *mr = NULL;
+
+	mr = vmalloc(sizeof(*mr));
+	if (mr == NULL)
+		return NULL;
+
+
+	mr->addrs = vmalloc(npages * sizeof(dma_addr_t));
+	if (!mr->addrs) {
+		nes_debug(NES_DBG_UD, "%s(%d) Cannot allocate mr struct "
+		"for %d pages\n", __func__, __LINE__, npages);
+		vfree(mr);
+		return NULL;
+	}
+	mr->pg_cnt = npages;
+	mr->in_use = 1;
+
+	INIT_LIST_HEAD(&mr->list);
+
+	return mr;
+}
+
+static void nes_ud_free_mr(struct nes_ud_mem_region *mr)
+{
+	if (mr->addrs != NULL)
+		vfree(mr->addrs);
+
+	vfree(mr);
+}
+
+/* nes_ud_get_hash_entry()
+ *
+ * function returns a key for hash table
+ */
+static inline
+int nes_ud_get_hash_entry(u32 stag)
+{
+	return stag & 0xff;
+}
+
+
+/* nes_ud_lookup_mr()
+ *
+ * function returns a pointer to mr realized by specific STAG
+ */
+static inline
+struct nes_ud_mem_region *nes_ud_lookup_mr(u32 stag)
+{
+	int key;
+	struct nes_ud_mem_region *mr;
+
+	key = nes_ud_get_hash_entry(stag);
+
+	mutex_lock(&ud_mem.mutex);
+	list_for_each_entry(mr, &ud_mem.mrs[key].list, list) {
+		ud_mem.mrs[key].read_stats++;
+		if (mr->stag == stag) {
+			mutex_unlock(&ud_mem.mutex);
+			return mr;
+		}
+	}
+	mutex_unlock(&ud_mem.mutex);
+	return NULL;
+}
+
+/* nes_ud_add_mr_hash()
+ *
+ * the function inserts the mr entry into the hash list
+ * the stag is a key
+ */
+static inline
+int nes_ud_add_mr_hash(struct nes_ud_mem_region *mr)
+{
+	int key;
+
+	/* first check if the stag is unique */
+	if (nes_ud_lookup_mr(mr->stag) != NULL) {
+		nes_debug(NES_DBG_UD, "%s(%d) double STAG error stag=%x\n",
+			__func__, __LINE__, mr->stag);
+		return -1;
+	}
+	key = nes_ud_get_hash_entry(mr->stag);
+
+	/* structure is global so mutexes are necessary */
+	mutex_lock(&ud_mem.mutex);
+
+	/* add mr to the list at start  */
+	list_add(&mr->list, &ud_mem.mrs[key].list);
+
+	mutex_unlock(&ud_mem.mutex);
+
+	return 0;
+
+}
+
+/* nes_ud_del_mr()
+ *
+ * the function removes the entry from the hash list
+ * the stag is the key
+ */
+static inline
+void nes_ud_del_mr(struct nes_ud_mem_region *mr)
+{
+	/* structure is global so mutexes are necessary */
+	mutex_lock(&ud_mem.mutex);
+
+	list_del(&mr->list);
+
+	/* init entry */
+	INIT_LIST_HEAD(&mr->list);
+
+	mutex_unlock(&ud_mem.mutex);
+}
+
+/* nes_ud_cleanup_mr()
+ *
+ * function deletes and and frees all hash entries
+ */
+static inline
+void nes_ud_cleanup_mr(void)
+{
+	struct nes_ud_mem_region *mr;
+	struct nes_ud_mem_region *next;
+	int i;
+
+	/* structure is global so mutexes are necessary */
+	mutex_lock(&ud_mem.mutex);
+
+	for (i = 0; i < NES_UD_MAX_REG_HASH_CNT; i++) {
+		if (list_empty(&ud_mem.mrs[i].list))
+			continue;
+
+		list_for_each_entry_safe(mr, next, &ud_mem.mrs[i].list, list) {
+			nes_debug(NES_DBG_UD, "%s(%d) non free stag=%x\n",
+				__func__, __LINE__, mr->stag);
+			list_del_init(&mr->list);
+
+			nes_ud_free_mr(mr);
+		}
+	}
+
+	mutex_unlock(&ud_mem.mutex);
+}
+
+u32 nes_ud_reg_mr(struct ib_umem *region, u64 length, u64 virt, u32 stag)
+{
+	unsigned long npages =
+		PAGE_ALIGN(region->length + region->offset) >> PAGE_SHIFT;
+	struct nes_ud_mem_region *mr = nes_ud_allocate_mr(npages);
+	struct ib_umem_chunk *chunk;
+	dma_addr_t page;
+	u32 chunk_pages = 0;
+	int nmap_index;
+	int i = 0;
+	int mr_id = 0;
+	nes_debug(NES_DBG_UD, "%s(%d) mr=%p length=%d virt=%p\n",
+		__func__, __LINE__, mr, (int)length, (void *)virt);
+	if (!mr)
+		return 0;
+
+
+	mr->stag = stag;
+
+	mr->va = virt;
+	mr->length = length;
+	list_for_each_entry(chunk, &region->chunk_list, list) {
+	for (nmap_index = 0; nmap_index < chunk->nmap; ++nmap_index) {
+		page = sg_dma_address(&chunk->page_list[nmap_index]);
+		chunk_pages = sg_dma_len(&chunk->page_list[nmap_index]) >> 12;
+		if (page & ~PAGE_MASK)
+			goto reg_user_mr_err;
+		if (!chunk_pages)
+			goto reg_user_mr_err;
+
+		for (i = 0; i < chunk_pages; i++) {
+			mr->addrs[mr_id] = page;
+			page += PAGE_SIZE;
+			if (++mr_id > npages)
+				goto reg_user_mr_err;
+			}
+		}
+	}
+	nes_debug(NES_DBG_UD, "%s(%d) stag=0x%x mr_id=%d npages=%d\n",
+		__func__, __LINE__, stag, mr_id, (int)npages);
+	nes_ud_add_mr_hash(mr);
+	return stag;
+
+reg_user_mr_err:
+	if (mr)
+		nes_ud_free_mr(mr);
+
+	return 0;
+}
+
+
+int nes_ud_dereg_mr(u32 stag)
+{
+	struct nes_ud_mem_region *mr = NULL;
+
+	nes_debug(NES_DBG_UD, "%s(%d) stag=0x%x\n", __func__, __LINE__, stag);
+
+	mr = nes_ud_lookup_mr(stag);
+	if (mr != NULL) {
+		nes_ud_del_mr(mr);
+		nes_ud_free_mr(mr);
+	} else {
+		nes_debug(NES_DBG_UD, "%s(%d) unknown stag=0x%x\n",
+		__func__, __LINE__, stag);
+	}
+
+	nes_debug(NES_DBG_UD, "%s(%d) done\n", __func__, __LINE__);
+	return 0;
+}
+
+
+int nes_ud_unsubscribe_mcast(struct nes_ud_file *file, union ib_gid *gid)
+{
+	int ret = 0;
+	int i;
+	struct nes_ud_resources *pRsc;
+
+	if (file->queue_type == NES_UD_SEND_QUEUE)
+		return -EFAULT;
+
+	pRsc = locate_ud_adapter(file->nesvnic->nesdev->nesadapter);
+	if (pRsc == NULL)
+		return -EFAULT;
+
+	for (i = 0; i < NES_UD_MCAST_TBL_SZ; i++) {
+		if (pRsc->mcast[i].in_use &&
+			pRsc->mcast[i].owner == file &&
+			pRsc->mcast[i].addr[0] == gid->raw[13] &&
+			pRsc->mcast[i].addr[1] == gid->raw[14] &&
+			pRsc->mcast[i].addr[2] == gid->raw[15]) {
+				pRsc->mcast[i].in_use = 0;
+				goto out;
+		}
+	}
+
+	ret = -EFAULT;
+out:
+	nes_debug(NES_DBG_UD, "%s(%d) %2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X \
+		ret=%d mcast=%d\n", __func__, __LINE__, gid->raw[10],
+		gid->raw[11], gid->raw[12], gid->raw[13], gid->raw[14],
+		gid->raw[15], ret , i);
+	return ret;
+
+}
+
+/* function returns a number of allocated multicast entries in given adapter */
+static int get_mcast_number_alloced(struct nes_ud_resources *pRsc)
+{
+	int i;
+	int no = 0;
+
+	for (i = 0; i < NES_UD_MCAST_TBL_SZ; i++) {
+		if (pRsc->mcast[i].in_use != 0)
+			no++;
+
+	}
+	return no;
+}
+
+/* function subscribe a multicast group in the system - PFT modification */
+int nes_ud_subscribe_mcast(struct nes_ud_file *file, union ib_gid *gid)
+{
+	struct nes_device *nesdev = file->nesvnic->nesdev;
+	int ret = 0;
+	int i;
+	__u8 hash_idx = 0;
+	__u8 instance = file->nes_ud_nic_index & 0x1;
+	unsigned addr = 0;
+	unsigned mqueue_ind_tbl;
+	struct nes_ud_resources *pRsc;
+
+	struct net_device *netdev = file->nesvnic->netdev;
+	struct netdev_hw_addr *ha;
+	int	multicast_address_exist = 0;
+
+
+	if (file->queue_type == NES_UD_SEND_QUEUE)
+		return -EFAULT;
+
+	pRsc = locate_ud_adapter(nesdev->nesadapter);
+	if (pRsc == NULL)
+		return -EFAULT;
+
+	netdev_for_each_mc_addr(ha, netdev) {
+		if ((ha->addr[3] == gid->raw[13]) &&
+		(ha->addr[4] == gid->raw[14]) &&
+		(ha->addr[5] == gid->raw[15]) &&
+		(ha->addr[0] == 0x01) &&
+		(ha->addr[1] == 0) &&
+		(ha->addr[2] == 0x5e)) {
+			multicast_address_exist = 1;
+			break;
+		}
+	}
+
+	if (multicast_address_exist == 0) {
+		nes_debug(NES_DBG_UD, "WARNING: multicast address not exist "
+			"on multicast list\n");
+		return -EFAULT;
+	}
+
+	/* first check that we have not subecribed to this mcast address, yet */
+	for (i = 0; i < NES_UD_MCAST_TBL_SZ; i++) {
+		if ((pRsc->mcast[i].in_use > 0) &&
+		    (pRsc->mcast[i].addr[0] == gid->raw[13]) &&
+		    (pRsc->mcast[i].addr[1] == gid->raw[14]) &&
+		    (pRsc->mcast[i].addr[2] == gid->raw[15])) {
+			if (pRsc->mcast[i].owner == file) {
+				nes_debug(NES_DBG_UD, "WARNING - subscribing  "
+				"mcast to the same nes_ud more than once\n");
+				break;
+			} else {
+		/* receiving the same multicast on different NICs is allowed:
+			1. when two different NICS are used
+			2.  exactly one QP exists on this adapter
+			3. The existing QP was allocated as first
+				or the second in the system
+		  */
+			if (pRsc->mcast[i].owner->nes_ud_nic_index !=
+						file->nes_ud_nic_index) {
+				if (get_mcast_number_alloced(pRsc) == 1) {
+					if ((i == 0) || (i == 1)) {
+						/* add the mask of other nics
+						that subscribe this address  */
+						break;
+					}
+				}
+			}
+			nes_debug(NES_DBG_UD, "ERROR - subscribing same mcast "
+				"to the diff nes_ud's and NIC  owner_idx = %d "
+				"file_idx = %d\n",
+				pRsc->mcast[i].owner->nes_ud_nic_index,
+				file->nes_ud_nic_index);
+			ret = -EFAULT;
+		    }
+		    goto out;
+		}
+	}
+
+	for (i = 0; i < NES_UD_MCAST_TBL_SZ; i++) {
+		if (!pRsc->mcast[i].in_use) {
+			pRsc->mcast[i].addr[0] = gid->raw[13];
+			pRsc->mcast[i].addr[1] = gid->raw[14];
+			pRsc->mcast[i].addr[2] = gid->raw[15];
+			pRsc->mcast[i].owner = file;
+			pRsc->mcast[i].in_use = 1;
+
+			hash_idx =
+				nes_ud_calculate_hash(pRsc->mcast[i].addr[2]);
+
+			addr = 0x6420 + ((hash_idx >> 3) << 2) + instance*0x100;
+			mqueue_ind_tbl = nes_read_indexed(nesdev, addr);
+			if (file->prio == NES_UD_DEV_PRIO_HIGH)
+				mqueue_ind_tbl &= ~(1 << ((hash_idx & 0x7)*4));
+			else
+				mqueue_ind_tbl |= 1 << ((hash_idx & 0x7)*4);
+
+			nes_write_indexed(nesdev, addr, mqueue_ind_tbl);
+			mqueue_ind_tbl = nes_read_indexed(nesdev, addr);
+
+			nes_debug(NES_DBG_UD, "%s(%d) addr=0x%x "
+				 "mqueue_ind_tbl=0x%x hash=0x%x, mac=0x%x\n",
+				__func__, __LINE__, addr, mqueue_ind_tbl,
+				hash_idx, pRsc->mcast[i].addr[2]);
+			/* take care of the case when linux join_mcast
+			is called before mcast_attach in that case our pft
+			will already be programmed with that mcast address,
+			just with wrong NIC we need just to find an address,
+			and fix the NIC additionally the mask with other NICs
+			that subscribed the address are added*/
+
+			mcast_fix_filter_table(file);
+			goto out;
+		}
+	}
+	ret = -EFAULT;
+
+out:
+
+	nes_debug(NES_DBG_UD, "%s(%d) %2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X \
+		ret=%d\n", __func__, __LINE__, gid->raw[10], gid->raw[11],
+		gid->raw[12], gid->raw[13], gid->raw[14], gid->raw[15], ret);
+
+	return ret;
+}
+
+
+static inline
+int nes_ud_post_recv(struct nes_ud_file *file,
+			u32 adap_no,
+			struct nes_ud_recv_wr *nes_ud_wr)
+{
+	struct nes_hw_nic_rq_wqe *nic_rqe;
+	struct nes_hw_nic_rq_wqe *rq_vbase =
+				(struct nes_hw_nic_rq_wqe *)file->wq_vbase;
+	struct nes_device *nesdev = file->nesvnic->nesdev;
+	u16 *wqe_fragment_length = NULL;
+	u32 mr_offset;
+	u32 page_offset;
+	u32 page_id;
+	struct nes_ud_mem_region *mr = NULL;
+	int remaining_length = 0;
+	int wqe_fragment_index = 0;
+	int err = 0;
+	int i = 0;
+	struct nes_ud_resources *pRsc;
+
+	/* check if qp is activated */
+	if (file->active == 0)
+		return -EFAULT;
+
+	pRsc = &nes_ud_rsc[adap_no];
+
+	/* let's assume for now that max sge count is 1 */
+	for (i = 0; i < nes_ud_wr->wr_cnt; i++) {
+		nic_rqe = &rq_vbase[file->head];
+
+		mr = nes_ud_lookup_mr(nes_ud_wr->sg_list[i].lkey);
+		if (mr == NULL)
+			return -EFAULT;
+
+
+		if (mr->va > nes_ud_wr->sg_list[i].addr ||
+		    (nes_ud_wr->sg_list[i].addr + nes_ud_wr->sg_list[i].length >
+							mr->va + mr->length)) {
+			err = -EFAULT;
+			goto out;
+		}
+
+		mr_offset = nes_ud_wr->sg_list[i].addr - mr->va;
+		page_offset = nes_ud_wr->sg_list[i].addr & ~PAGE_MASK;
+		page_id = ((mr->va & ~PAGE_MASK) + mr_offset) >> PAGE_SHIFT;
+
+		wqe_fragment_length =
+		(u16 *)&nic_rqe->wqe_words[NES_NIC_RQ_WQE_LENGTH_1_0_IDX];
+
+		remaining_length = nes_ud_wr->sg_list[i].length;
+		wqe_fragment_index = 0;
+
+		while (remaining_length > 0) {
+			if (wqe_fragment_index >= 4) {
+				err = -EFAULT;
+				goto out;
+			}
+
+			set_wqe_64bit_value(nic_rqe->wqe_words,
+			NES_NIC_RQ_WQE_FRAG0_LOW_IDX + 2*wqe_fragment_index,
+				  mr->addrs[page_id]+page_offset);
+
+		      wqe_fragment_length[wqe_fragment_index] =
+				cpu_to_le16(PAGE_SIZE - page_offset);
+
+		      remaining_length -= PAGE_SIZE - page_offset;
+		      page_offset = 0;
+		      page_id++;
+		      wqe_fragment_index++;
+		}
+
+		nes_write32(nesdev->regs+NES_WQE_ALLOC, (1 << 24) |  file->qpn);
+
+		file->head = (file->head+1) & ~NES_NIC_WQ_SIZE;
+	}
+out:
+	return err;
+}
+
+static inline
+int nes_ud_post_send(struct nes_ud_file *file,
+			u32 adap_no,
+			struct nes_ud_send_wr *nes_ud_wr)
+{
+	struct nes_hw_nic_sq_wqe *nic_sqe;
+	struct nes_hw_nic_sq_wqe *sq_vbase =
+			(struct nes_hw_nic_sq_wqe *)file->wq_vbase;
+	struct nes_device *nesdev = file->nesvnic->nesdev;
+	u16 *wqe_fragment_length = NULL;
+	u32 mr_offset;
+	u32 page_offset;
+	u32 page_id;
+	struct nes_ud_mem_region *mr = NULL;
+	int remaining_length = 0;
+	int wqe_fragment_index = 0;
+	int err = 0;
+	int misc_flags = NES_NIC_SQ_WQE_COMPLETION;
+	int i = 0;
+	struct nes_ud_resources *pRsc;
+
+	/* check if qp is activated */
+	if (file->active == 0)
+		return -EFAULT;
+
+	pRsc = &nes_ud_rsc[adap_no];
+
+	/* check if is not set checksum */
+	if (!(nes_ud_wr->flags & IB_SEND_IP_CSUM))
+		misc_flags |= NES_NIC_SQ_WQE_DISABLE_CHKSUM;
+
+	/* let's assume for now that max sge count is 1 */
+	for (i = 0; i < nes_ud_wr->wr_cnt; i++) {
+		nic_sqe = &sq_vbase[file->head];
+
+		mr = nes_ud_lookup_mr(nes_ud_wr->sg_list[i].lkey);
+		if (mr == NULL)
+			return -EFAULT;
+
+
+		if ((mr->va > nes_ud_wr->sg_list[i].addr) ||
+		     (nes_ud_wr->sg_list[i].addr+nes_ud_wr->sg_list[i].length >
+							mr->va + mr->length)) {
+
+			err = -EFAULT;
+			goto out;
+		}
+
+		mr_offset = nes_ud_wr->sg_list[i].addr - mr->va;
+		page_offset = nes_ud_wr->sg_list[i].addr & ~PAGE_MASK;
+		page_id = ((mr->va & ~PAGE_MASK) + mr_offset) >> PAGE_SHIFT;
+
+		wqe_fragment_length =
+		(u16 *)&nic_sqe->wqe_words[NES_NIC_SQ_WQE_LENGTH_0_TAG_IDX];
+
+		wqe_fragment_length++; /* skip vlan tag */
+		remaining_length = nes_ud_wr->sg_list[i].length;
+		wqe_fragment_index = 0;
+
+		while (remaining_length > 0) {
+			if (wqe_fragment_index >= 4) {
+				err = -EFAULT;
+				goto out;
+			}
+			set_wqe_64bit_value(nic_sqe->wqe_words,
+				NES_NIC_SQ_WQE_FRAG0_LOW_IDX +
+						2*wqe_fragment_index,
+				mr->addrs[page_id]+page_offset);
+		      wqe_fragment_length[wqe_fragment_index] =
+				cpu_to_le16(PAGE_SIZE - page_offset);
+		      remaining_length -= PAGE_SIZE - page_offset;
+		      page_offset = 0;
+		      page_id++;
+		      wqe_fragment_index++;
+		}
+		nic_sqe->wqe_words[NES_IWARP_SQ_WQE_TOTAL_PAYLOAD_IDX] =
+				cpu_to_le32(nes_ud_wr->sg_list[i].length);
+		nic_sqe->wqe_words[NES_NIC_SQ_WQE_MISC_IDX] =
+						cpu_to_le32(misc_flags);
+
+		nes_write32(nesdev->regs+NES_WQE_ALLOC,
+					(1 << 24) | (1 << 23) | file->qpn);
+
+		file->head = (file->head+1) & ~NES_NIC_WQ_SIZE;
+	}
+out:
+	return err;
+}
+
+
+
+static void nes_ud_mcast_cleanup_work(struct nes_ud_file *file)
+{
+	int i = 0;
+	int num_queues = count_files_by_nic(file->nesvnic, file->queue_type);
+	struct nes_ud_resources *pRsc;
+
+	pRsc = locate_ud_adapter(file->nesvnic->nesdev->nesadapter);
+	if (pRsc == NULL)
+		return;
+
+
+	nes_debug(NES_DBG_UD, "%s(%d) file->rsc_idx=%d\n",
+			__func__, __LINE__, file->rsc_idx);
+
+	mutex_lock(&pRsc->mutex);
+	for (i = 0; i < NES_UD_MCAST_TBL_SZ; i++) {
+		if (pRsc->mcast[i].owner == file) {
+			nes_debug(NES_DBG_UD, "%s(%d) mcast cleared idx=%d "
+				"%2.2X:%2.2X:%2.2X\n", __func__, __LINE__,
+				i, pRsc->mcast[i].addr[0],
+				pRsc->mcast[i].addr[1],
+				pRsc->mcast[i].addr[2]);
+
+			pRsc->mcast[i].in_use = 0;
+			remove_mcast_from_pft(file, pRsc->mcast[i].addr);
+		}
+	}
+
+	if (del_rsc_list(file) == 0) {
+		if (num_queues == 1)
+			file->nesvnic->mcrq_mcast_filter = NULL;
+
+	}
+	mutex_unlock(&pRsc->mutex);
+}
+
+struct nes_ud_file *nes_ud_create_wq(struct nes_vnic *nesvnic, int isrecv)
+{
+	struct nes_ud_file *file;
+	int ret = 0;
+	file = nes_ud_get_nxt_channel(nesvnic, (isrecv) ?
+				NES_UD_RECV_QUEUE : NES_UD_SEND_QUEUE);
+	if (!file)
+		return NULL;
+
+
+	ret =  nes_ud_init_channel(file);
+	if (ret != 0) {
+		del_rsc_list(file);
+		return NULL;
+	}
+
+	dev_hold(file->nesvnic->netdev);
+
+	nes_debug(NES_DBG_UD, "%s(%d) file=%p\n", __func__, __LINE__, file);
+	return file;
+}
+
+
+
+int nes_ud_destroy_wq(struct nes_ud_file *file)
+{
+	struct nes_ud_resources *pRsc;
+	int count = 0;
+	int i;
+	pRsc = locate_ud_adapter(file->nesvnic->nesdev->nesadapter);
+	if (pRsc == NULL)
+		return -EFAULT;
+
+	if (file->active) {
+		nes_ud_mcast_cleanup_work(file);
+		nes_ud_free_resources(file);
+	}
+
+	/* check if the the adapter has any queues */
+	for (i = 0; i < NES_UD_MAX_NIC_CNT; i++) {
+		if (pRsc->nics[i].file->active != 0)
+			count++;
+
+	}
+	if (count == 0) {
+		nes_debug(NES_DBG_UD, "%s(%d) adapter %d "
+				"is ready to next use\n",
+				__func__, __LINE__, pRsc->adapter_no);
+		pRsc->pAdap = NULL;
+	}
+	nes_debug(NES_DBG_UD, "%s(%d) done\n", __func__, __LINE__);
+	return 0;
+}
+
+
+struct nes_ud_sksq_file {
+	unsigned long shared_page;
+	struct nes_ud_file *nes_ud_send_file;
+	struct nes_ud_file *nes_ud_recv_file;
+};
+
+static ssize_t nes_ud_sksq_write(struct file *filp, const char __user *buf,
+		size_t len, loff_t *pos)
+{
+	struct nes_ud_sksq_file *file = filp->private_data;
+	struct nes_ud_send_wr *nes_ud_wr =
+			(struct nes_ud_send_wr *)file->shared_page;
+	u32 adap_no;
+	u32 nic_no;
+
+	nic_no = ((nes_ud_wr->qpn >> 16) & 0x0f00) >> 8;
+	adap_no = ((nes_ud_wr->qpn >> 16) & 0xf000) >> 12;
+	if (unlikely(!file->nes_ud_send_file)) {
+		struct nes_ud_file *nes_ud_file = NULL;
+
+	nes_ud_file = nes_ud_rsc[adap_no].nics[nic_no].file;
+	/* the nic must be active and previously activated */
+	if ((nes_ud_file->active == 0) ||
+		(nes_ud_file->qpn != ((nes_ud_wr->qpn >> 16) & 0xff)))
+			return -EAGAIN;
+
+		file->nes_ud_send_file = nes_ud_file;
+		nes_debug(NES_DBG_UD, "send shared page addr = %p "
+				"adap_no = %d nic_no=%d qpn=%x\n",
+				nes_ud_wr, adap_no, nic_no, nes_ud_wr->qpn);
+  }
+  return nes_ud_post_send(file->nes_ud_send_file, adap_no, nes_ud_wr);
+
+}
+
+static ssize_t nes_ud_sksq_read(struct file *filp, char __user *buf,
+		size_t len, loff_t *pos)
+{
+	struct nes_ud_sksq_file *file = filp->private_data;
+	struct nes_ud_recv_wr *nes_ud_recv_wr;
+	u32 adap_no;
+	u32 nic_no;
+
+	nes_ud_recv_wr = (struct nes_ud_recv_wr *)(file->shared_page+2048);
+	adap_no = (nes_ud_recv_wr->qpn & 0xf000) >> 12;
+	nic_no = (nes_ud_recv_wr->qpn & 0x0f00) >> 8;
+
+	if (unlikely(!file->nes_ud_recv_file)) {
+		struct nes_ud_file *nes_ud_file = NULL;
+
+		nes_ud_file = nes_ud_rsc[adap_no].nics[nic_no].file;
+		/* the nic must be active and previously activated */
+		if ((nes_ud_file->active == 0) ||
+			(nes_ud_file->qpn != (nes_ud_recv_wr->qpn & 0xff)))
+				return -EAGAIN;
+
+		file->nes_ud_recv_file  = nes_ud_file;
+		nes_debug(NES_DBG_UD, "recv shared page addr = %p "
+				"adap_no = %d nic_no=%d qpn=%x\n",
+			nes_ud_recv_wr, adap_no, nic_no, nes_ud_recv_wr->qpn);
+	}
+	return nes_ud_post_recv(file->nes_ud_recv_file,
+				adap_no, nes_ud_recv_wr);
+}
+
+static int nes_ud_sksq_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	struct nes_ud_sksq_file *file = filp->private_data;
+
+	nes_debug(NES_DBG_UD, "shared mem pgprot_val(prot)=0x%x pa=%p\n",
+			(unsigned int)pgprot_val(vma->vm_page_prot),
+			(void *)virt_to_phys((void *)file->shared_page));
+	if (remap_pfn_range(vma, vma->vm_start,
+			virt_to_phys((void *)file->shared_page) >> PAGE_SHIFT,
+			 PAGE_SIZE, vma->vm_page_prot)) {
+		printk(KERN_ERR "remap_pfn_range failed.\n");
+		return -EAGAIN;
+	}
+	return 0;
+}
+
+
+static int nes_ud_sksq_open(struct inode *inode, struct file *filp)
+{
+	struct nes_ud_sksq_file *file;
+
+	file = kmalloc(sizeof *file, GFP_KERNEL);
+	if (!file)
+		return -ENOMEM;
+
+	memset(file, 0, sizeof *file);
+	nes_debug(NES_DBG_UD, "%s(%d) file=%p\n",
+			__func__, __LINE__, file);
+
+	filp->private_data = file;
+	file->nes_ud_send_file = NULL;
+	file->nes_ud_recv_file = NULL;
+
+	file->shared_page = __get_free_page(GFP_USER);
+	return 0;
+}
+
+static int nes_ud_sksq_close(struct inode *inode, struct file *filp)
+{
+
+	struct nes_ud_sksq_file *file = filp->private_data;
+
+	if (file->shared_page) {
+		free_page(file->shared_page);
+		file->shared_page = 0;
+	}
+	kfree(file);
+	return 0;
+}
+
+static const struct file_operations nes_ud_sksq_fops = {
+	.owner = THIS_MODULE,
+	.open = nes_ud_sksq_open,
+	.release = nes_ud_sksq_close,
+	.write = nes_ud_sksq_write,
+	.read = nes_ud_sksq_read,
+	.mmap = nes_ud_sksq_mmap,
+};
+
+
+static struct miscdevice nes_ud_sksq_misc = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "nes_ud_sksq",
+	.fops = &nes_ud_sksq_fops,
+};
+
+/*
+ * function replaces the CQ pointer in QP stored in the file
+ * the QP must have a valid CQ pointers assotiated with it
+ */
+int nes_ud_cq_replace(struct nes_vnic *nesvnic, struct nes_cq *cq)
+{
+	u32 cq_num;
+	struct nes_ud_file *file;
+	struct nes_ud_resources *pRsc;
+
+	BUG_ON(!cq);
+
+	pRsc = locate_ud_adapter(nesvnic->nesdev->nesadapter);
+	if (pRsc == NULL)
+		return -EFAULT;
+
+
+	/* now create a QP number on base cq and adapter no */
+	cq_num = cq->hw_cq.cq_number;
+
+	nes_debug(NES_DBG_UD, "%s(%d) cq_number=%d\n",
+			__func__, __LINE__, cq_num);
+
+	/* the QP number should have the same number like CQ number */
+	file = get_file_by_qpn(pRsc, cq_num);
+	if (!file) {
+		nes_debug(NES_DBG_UD, "%s(%d) file not found\n",
+				__func__, __LINE__);
+		return -EFAULT;
+	}
+	if (file->qp_ptr) {
+		if (file->queue_type == NES_UD_RECV_QUEUE) {
+			nes_debug(NES_DBG_UD, "%s(%d) RECV file found "
+			"old=%p new=%p\n", __func__, __LINE__,
+			file->qp_ptr->ibqp.recv_cq, cq);
+			file->qp_ptr->ibqp.recv_cq = &cq->ibcq;
+		}
+		if (file->queue_type == NES_UD_SEND_QUEUE) {
+			nes_debug(NES_DBG_UD, "%s(%d) SEND file found "
+			"old=%p new=%p\n", __func__, __LINE__,
+			file->qp_ptr->ibqp.send_cq, cq);
+
+			file->qp_ptr->ibqp.send_cq = &cq->ibcq;
+		}
+	}
+	return 0;
+}
+int nes_ud_init(void)
+{
+	int i = 0;
+	int adap_no;
+	struct nes_ud_resources *pRsc;
+
+	nes_debug(NES_DBG_UD, "%s(%d)\n", __func__, __LINE__);
+
+	/* the memory registration is global for all NICS */
+	memset(&ud_mem, 0, sizeof(ud_mem));
+
+	/* init hash list of memory entries */
+	for (i = 0; i < NES_UD_MAX_REG_HASH_CNT; i++) {
+		INIT_LIST_HEAD(&ud_mem.mrs[i].list);
+		ud_mem.mrs[i].read_stats = 0;
+	}
+	mutex_init(&ud_mem.mutex);
+
+	/*allocate resources fro each adapter */
+	for (adap_no = 0; adap_no < NES_UD_MAX_ADAPTERS; adap_no++) {
+		pRsc = &nes_ud_rsc[adap_no];
+
+		memset(pRsc, 0, sizeof(*pRsc));
+
+		mutex_init(&pRsc->mutex);
+
+		pRsc->adapter_no = adap_no;
+		pRsc->pAdap = NULL;
+
+		pRsc->num_logport_confed = 0;
+		pRsc->num_allocated_nics = 0;
+		pRsc->logport_2_map = 0xf;
+		pRsc->logport_3_map = 0xf;
+		for (i = 0; i < NES_UD_MCAST_TBL_SZ; i++)
+			pRsc->mcast[i].in_use = 0;
+
+		pRsc->nics[0].qpn = 20;
+		pRsc->nics[0].nic_index = 2;
+		pRsc->nics[0].logical_port = 2;
+		pRsc->nics[0].prio = NES_UD_DEV_PRIO_HIGH;
+		pRsc->nics[0].queue_type = NES_UD_RECV_QUEUE;
+		pRsc->nics[0].file = &pRsc->nics[0].file_body;
+
+		pRsc->nics[1].qpn = 22;
+		pRsc->nics[1].nic_index = 3;
+		pRsc->nics[1].logical_port = 3;
+		pRsc->nics[1].prio = NES_UD_DEV_PRIO_HIGH;
+		pRsc->nics[1].queue_type = NES_UD_RECV_QUEUE;
+		pRsc->nics[1].file = &pRsc->nics[1].file_body;
+
+		pRsc->nics[2].qpn = 21;
+		pRsc->nics[2].nic_index = 2;
+		pRsc->nics[2].logical_port = 2;
+		pRsc->nics[2].prio = NES_UD_DEV_PRIO_LOW;
+		pRsc->nics[2].queue_type = NES_UD_RECV_QUEUE;
+		pRsc->nics[2].file = &pRsc->nics[2].file_body;
+
+		pRsc->nics[3].qpn = 23;
+		pRsc->nics[3].nic_index = 3;
+		pRsc->nics[3].logical_port = 3;
+		pRsc->nics[3].prio = NES_UD_DEV_PRIO_LOW;
+		pRsc->nics[3].queue_type = NES_UD_RECV_QUEUE;
+		pRsc->nics[3].file = &pRsc->nics[3].file_body;
+
+		pRsc->nics[4].qpn = 26;
+		pRsc->nics[4].nic_index = 6;
+		pRsc->nics[4].logical_port = 2;
+		pRsc->nics[4].prio = NES_UD_DEV_PRIO_HIGH;
+		pRsc->nics[4].queue_type = NES_UD_SEND_QUEUE;
+		pRsc->nics[4].file = &pRsc->nics[4].file_body;
+
+		pRsc->nics[5].qpn = 27;
+		pRsc->nics[5].nic_index = 7;
+		pRsc->nics[5].logical_port = 3;
+		pRsc->nics[5].prio = NES_UD_DEV_PRIO_HIGH;
+		pRsc->nics[5].queue_type = NES_UD_SEND_QUEUE;
+		pRsc->nics[5].file = &pRsc->nics[5].file_body;
+
+		pRsc->nics[6].qpn = 30;
+		pRsc->nics[6].nic_index = 10;
+		pRsc->nics[6].logical_port = 2;
+		pRsc->nics[6].prio = NES_UD_DEV_PRIO_LOW;
+		pRsc->nics[6].queue_type = NES_UD_SEND_QUEUE;
+		pRsc->nics[6].file = &pRsc->nics[6].file_body;
+
+		pRsc->nics[7].qpn = 31;
+		pRsc->nics[7].nic_index = 11;
+		pRsc->nics[7].logical_port = 3;
+		pRsc->nics[7].prio = NES_UD_DEV_PRIO_LOW;
+		pRsc->nics[7].queue_type = NES_UD_SEND_QUEUE;
+		pRsc->nics[7].file = &pRsc->nics[7].file_body;
+
+	}
+	nes_ud_workqueue = create_singlethread_workqueue("nes_ud");
+
+	return misc_register(&nes_ud_sksq_misc);
+}
+
+
+int nes_ud_exit(void)
+{
+	/* clean memory hash list */
+	nes_ud_cleanup_mr();
+	misc_deregister(&nes_ud_sksq_misc);
+	return 0;
+}
+
diff --git a/drivers/infiniband/hw/nes/nes_ud.h b/drivers/infiniband/hw/nes/nes_ud.h
new file mode 100644
index 0000000..5a03b33
--- /dev/null
+++ b/drivers/infiniband/hw/nes/nes_ud.h
@@ -0,0 +1,86 @@
+/*
+ * Copyright (c) 2008 - 2010 Intel Corporation.  All rights reserved.
+ * Copyright (c) 2006 - 2008 Neteffect, All rights reserved.
+ * Copyright (c) 2005 Open Grid Computing, Inc. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+#ifndef __NES_UD_H
+#define __NES_UD_H
+
+enum nes_ud_dev_priority {
+	NES_UD_DEV_PRIO_HIGH,
+	NES_UD_DEV_PRIO_LOW,
+};
+
+enum nes_ud_queue_type {
+  NES_UD_RECV_QUEUE,
+  NES_UD_SEND_QUEUE,
+};
+
+enum nes_ud_mcast_mode {
+  NES_UD_MCAST_ALL_MODE,
+  NES_UD_MCAST_PFT_MODE,
+};
+
+
+struct nes_ud_file {
+	struct nes_vnic *nesvnic;
+	u8 active;
+	char ifrn_name[IFNAMSIZ];
+	int nes_ud_nic_index;
+	int qpn;
+	enum nes_ud_dev_priority prio;
+	enum nes_ud_mcast_mode mcast_mode;
+	enum nes_ud_queue_type queue_type;
+	void      *nic_vbase;
+	dma_addr_t nic_pbase;
+	int        nic_mem_size;
+	void      *wq_vbase;
+	dma_addr_t wq_pbase;
+	int mss;
+	struct delayed_work mcast_cleanup_work;
+	int head;
+	int tail;
+	u32 rsc_idx;
+	struct nes_qp *qp_ptr; /* it is association used for CQ replacement */
+	u32 adapter_no;    /* assotiation to allocated adapter */
+};
+
+int nes_ud_init(void);
+int nes_ud_exit(void);
+struct nes_ud_file *nes_ud_create_wq(struct nes_vnic *nesvnic, int isrecv);
+int nes_ud_destroy_wq(struct nes_ud_file *file);
+u32 nes_ud_reg_mr(struct ib_umem *region, u64 length, u64 virt, u32 stag);
+int nes_ud_dereg_mr(u32 stag);
+int nes_ud_subscribe_mcast(struct nes_ud_file *file, union ib_gid *gid);
+int nes_ud_unsubscribe_mcast(struct nes_ud_file *file, union ib_gid *gid);
+int nes_ud_cq_replace(struct nes_vnic *nesvnic, struct nes_cq *cq);
+
+#endif
diff --git a/drivers/infiniband/hw/nes/nes_verbs.c b/drivers/infiniband/hw/nes/nes_verbs.c
index 9bc2d74..dafc783 100644
--- a/drivers/infiniband/hw/nes/nes_verbs.c
+++ b/drivers/infiniband/hw/nes/nes_verbs.c
@@ -46,6 +46,8 @@
 
 #include <rdma/ib_umem.h>
 
+#include "nes_ud.h"
+
 atomic_t mod_qp_timouts;
 atomic_t qps_created;
 atomic_t sw_qps_destroyed;
@@ -1139,7 +1141,6 @@ static struct ib_qp *nes_create_qp(struct ib_pd *ibpd,
 	if (init_attr->create_flags)
 		return ERR_PTR(-EINVAL);
 
-	atomic_inc(&qps_created);
 	switch (init_attr->qp_type) {
 		case IB_QPT_RC:
 			if (nes_drv_opt & NES_DRV_OPT_NO_INLINE_DATA) {
@@ -1405,10 +1406,122 @@ static struct ib_qp *nes_create_qp(struct ib_pd *ibpd,
 					nesqp->hwqp.qp_id, nesqp, (u32)sizeof(*nesqp));
 			spin_lock_init(&nesqp->lock);
 			nes_add_ref(&nesqp->ibqp);
+			/* moved here to be sure that QP is really created */
+			/*(now it counted a number of QP creation trials */
+			atomic_inc(&qps_created);
 			break;
-		default:
-			nes_debug(NES_DBG_QP, "Invalid QP type: %d\n", init_attr->qp_type);
-			return ERR_PTR(-EINVAL);
+
+	case IB_QPT_RAW_PACKET:
+	if (!ibpd->uobject)
+		return ERR_PTR(-EINVAL);
+
+	/* we are about to destroy those cqs w/o destroying qp
+	 now free memory for nespbl that is not used
+	 first map nespbl with the qp created */
+	if (ibpd->uobject->context) {
+		nes_ucontext = to_nesucontext(ibpd->uobject->context);
+		if (udata) {
+			if (ib_copy_from_udata(&req,
+				udata,
+				sizeof(struct nes_create_qp_req))) {
+				return ERR_PTR(-EFAULT);
+			}
+			if (req.user_wqe_buffers) {
+				err = 1;
+				list_for_each_entry(nespbl,
+				&nes_ucontext->qp_reg_mem_list,
+				list) {
+					if (nespbl->user_base ==
+						req.user_wqe_buffers) {
+						list_del(&nespbl->list);
+						err = 0;
+						/* done with memory allocated
+						during nes_reg_user_mr() */
+						pci_free_consistent(
+							nesdev->pcidev,
+							nespbl->pbl_size,
+							nespbl->pbl_vbase,
+							nespbl->pbl_pbase);
+						kfree(nespbl);
+						break;
+					}
+				}
+			}
+		}
+	}
+	/* Need 512 (actually now 1024) byte alignment on this structure */
+	mem = kzalloc(sizeof(*nesqp)+NES_SW_CONTEXT_ALIGN-1, GFP_KERNEL);
+	if (!mem) {
+		nes_debug(NES_DBG_UD, "Unable to allocate QP\n");
+		return ERR_PTR(-ENOMEM);
+	}
+	u64nesqp = (unsigned long)mem;
+	u64nesqp += ((u64)NES_SW_CONTEXT_ALIGN) - 1;
+	u64temp = ((u64)NES_SW_CONTEXT_ALIGN) - 1;
+	u64nesqp &= ~u64temp;
+	nesqp = (struct nes_qp *)(unsigned long)u64nesqp;
+	nesqp->allocated_buffer = mem;
+
+	nesqp->rx_ud_wq = nes_ud_create_wq(nesvnic, 1);
+	nesqp->tx_ud_wq = nes_ud_create_wq(nesvnic, 0);
+	if ((!nesqp->rx_ud_wq) || (!nesqp->tx_ud_wq)) {
+		kfree(nesqp->allocated_buffer);
+		return ERR_PTR(-EFAULT);
+	}
+
+	/* create association between qp and tx/rx files
+	 it is used when CQ is replaced from user space */
+	nesqp->rx_ud_wq->qp_ptr = nesqp;
+	nesqp->tx_ud_wq->qp_ptr = nesqp;
+
+	sq_size = init_attr->cap.max_send_wr;
+	rq_size = init_attr->cap.max_recv_wr;
+	nes_debug(NES_DBG_UD, "%s(%d) sq_size=%d rq_size=%d\n",
+				__func__,
+				__LINE__, sq_size, rq_size);
+	uresp.actual_sq_size = sq_size;
+	uresp.actual_rq_size = rq_size;
+
+	/* Init qp size due to ibv_query_qp requirements */
+	nesqp->hwqp.sq_size = sq_size;
+	nesqp->hwqp.rq_size = rq_size;
+
+	/* enhance the response qp number with adapter number and QP number
+	on this adapter
+	 user space will use this identifier when packets will be posted */
+	uresp.qp_id = nesqp->rx_ud_wq->qpn |
+			(nesqp->rx_ud_wq->adapter_no << 12) |
+			(nesqp->rx_ud_wq->rsc_idx << 8);
+	uresp.qp_id = uresp.qp_id |
+			((nesqp->tx_ud_wq->qpn |
+			(nesqp->tx_ud_wq->adapter_no << 12) |
+			(nesqp->tx_ud_wq->rsc_idx << 8)) << 16);
+
+	nesqp->hwqp.qp_id = uresp.qp_id;
+	nesqp->ibqp.qp_num = uresp.qp_id;
+
+	nes_debug(NES_DBG_UD, "%s(%d) qpid=0x%x\n",
+			__func__, __LINE__, uresp.qp_id);
+	if (ib_copy_to_udata(udata, &uresp, sizeof uresp)) {
+		kfree(nesqp->allocated_buffer);
+		return ERR_PTR(-EFAULT);
+	}
+	/* the usecount is decreased because without it
+	the cq re-creation in user-spce will fail */
+	atomic_dec(&init_attr->send_cq->usecnt);
+	atomic_dec(&init_attr->recv_cq->usecnt);
+	nes_add_ref(&nesqp->ibqp);
+	spin_lock_init(&nesqp->lock);
+
+	/* moved here to be sure that QP is really created
+	(now it counted a number of QP creation trials */
+	atomic_inc(&qps_created);
+	return &nesqp->ibqp;
+
+	default:
+		nes_debug(NES_DBG_QP, "Invalid QP type: %d\n",
+					init_attr->qp_type);
+		return ERR_PTR(-EINVAL);
 	}
 
 	nesqp->sig_all = (init_attr->sq_sig_type == IB_SIGNAL_ALL_WR);
@@ -1462,6 +1575,8 @@ static void nes_clean_cq(struct nes_qp *nesqp, struct nes_cq *nescq)
 static int nes_destroy_qp(struct ib_qp *ibqp)
 {
 	struct nes_qp *nesqp = to_nesqp(ibqp);
+	struct nes_cq *scq;
+	struct nes_cq *rcq;
 	struct nes_ucontext *nes_ucontext;
 	struct ib_qp_attr attr;
 	struct iw_cm_id *cm_id;
@@ -1471,6 +1586,39 @@ static int nes_destroy_qp(struct ib_qp *ibqp)
 	atomic_inc(&sw_qps_destroyed);
 	nesqp->destroyed = 1;
 
+	if (nesqp->ibqp.qp_type == IB_QPT_RAW_PACKET) {
+		/* check the QP refernece count */
+		if (atomic_read(&nesqp->refcount) == 0)
+			BUG();
+		if (atomic_dec_and_test(&nesqp->refcount)) {
+			/* destroy send and rcv  QPs */
+			if (nesqp->rx_ud_wq)
+				nes_ud_destroy_wq(nesqp->rx_ud_wq);
+			nesqp->rx_ud_wq = 0;
+
+			if (nesqp->tx_ud_wq)
+				nes_ud_destroy_wq(nesqp->tx_ud_wq);
+			nesqp->tx_ud_wq = 0;
+			atomic_inc(&qps_destroyed);
+
+			/* to prevent the destroy of cq before QP
+			destroy the usecount is used */
+			if (ibqp->send_cq) {
+				scq = to_nescq(ibqp->send_cq);
+				atomic_inc(&ibqp->send_cq->usecnt);
+				atomic_dec(&scq->usecnt);
+			}
+			if (ibqp->recv_cq) {
+				rcq = to_nescq(ibqp->recv_cq);
+				atomic_inc(&ibqp->recv_cq->usecnt);
+				atomic_dec(&rcq->usecnt);
+			}
+			/* free memory for the qp */
+			kfree(nesqp->allocated_buffer);
+		}
+		return 0;
+	}
+
 	/* Blow away the connection if it exists. */
 	if (nesqp->ibqp_state >= IB_QPS_INIT && nesqp->ibqp_state <= IB_QPS_RTS) {
 		/* if (nesqp->ibqp_state == IB_QPS_RTS) { */
@@ -1567,9 +1715,18 @@ static struct ib_cq *nes_create_cq(struct ib_device *ibdev, int entries,
 		return ERR_PTR(-ENOMEM);
 	}
 
+	/* to make sure that RAW ETH cq will be not destoyed
+	without qp destroy the internal usecount is used
+	 the ibcq usecount cannot be used because the  RAW ETH makes
+	recreation of the CQs after QP creation
+	 when this situation occured (mcrqf != 0) the usecount is increase
+	 the ibcq usecount is cleared after successfull CQ creation */
+	atomic_set(&nescq->usecnt, 0);
+
 	nescq->hw_cq.cq_size = max(entries + 1, 5);
 	nescq->hw_cq.cq_number = cq_num;
 	nescq->ibcq.cqe = nescq->hw_cq.cq_size - 1;
+	nescq->mcrqf = 0;
 
 
 	if (context) {
@@ -1586,8 +1743,23 @@ static struct ib_cq *nes_create_cq(struct ib_device *ibdev, int entries,
 				nescq->hw_cq.cq_number = nesvnic->nic.qp_id + 28 + 2 * ((nes_ucontext->mcrqf & 0xf) - 1);
 			else if (nes_ucontext->mcrqf & 0x40000000)
 				nescq->hw_cq.cq_number = nes_ucontext->mcrqf & 0xffff;
+			else if (nes_ucontext->mcrqf & 0x20000000) {
+				/* the cq number is coded
+						adapter:4/nic:4/cq_num:8 */
+				nescq->hw_cq.cq_number =
+						nes_ucontext->mcrqf & 0x00ff;
+
+				/* to prevent the cq destroy before qp destroy
+				the internal usecount is increased
+				in this place it is the  RAW ETH specific CQ
+				(after re-creation)
+				only  RAW ETH type QP destroy can decrease
+				this usecounter */
+				atomic_inc(&nescq->usecnt);
+			}
 			else
 				nescq->hw_cq.cq_number = nesvnic->mcrq_qp_id + nes_ucontext->mcrqf-1;
+
 			nescq->mcrqf = nes_ucontext->mcrqf;
 			nes_free_resource(nesadapter, nesadapter->allocated_cqs, cq_num);
 		}
@@ -1776,6 +1948,10 @@ static struct ib_cq *nes_create_cq(struct ib_device *ibdev, int entries,
 			kfree(nescq);
 			return ERR_PTR(-EFAULT);
 		}
+		if (nes_ucontext->mcrqf & 0x20000000) {
+			/* change the cq address only for  RAW in QP pointer */
+			nes_ud_cq_replace(nesvnic, nescq);
+		}
 	}
 
 	return &nescq->ibcq;
@@ -1805,6 +1981,11 @@ static int nes_destroy_cq(struct ib_cq *ib_cq)
 	nesdev = nesvnic->nesdev;
 	nesadapter = nesdev->nesadapter;
 
+	if (atomic_read(&nescq->usecnt) != 0) {
+		nes_debug(NES_DBG_CQ, "CQ is in use now. %d\n",
+				(int) atomic_read(&nescq->usecnt));
+		return -EBUSY;
+	}
 	nes_debug(NES_DBG_CQ, "Destroy CQ%u\n", nescq->hw_cq.cq_number);
 
 	/* Send DestroyCQ request to CQP */
@@ -2540,6 +2721,13 @@ static struct ib_mr *nes_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
 				nesmr->ibmr.lkey = stag;
 				nesmr->mode = IWNES_MEMREG_TYPE_MEM;
 				ibmr = &nesmr->ibmr;
+				/* register memory parallelly for RAW ETH */
+				if (nes_ud_reg_mr(region, length,
+						virt, stag) == 0) {
+					ib_umem_release(region);
+					kfree(nesmr);
+					ibmr = ERR_PTR(-ENOMEM);
+				}
 			} else {
 				ib_umem_release(region);
 				kfree(nesmr);
@@ -2733,6 +2921,9 @@ static int nes_dereg_mr(struct ib_mr *ib_mr)
 	}
 	nes_free_resource(nesadapter, nesadapter->allocated_mrs,
 			(ib_mr->rkey & 0x0fffff00) >> 8);
+	ret = nes_ud_dereg_mr(ib_mr->rkey);
+	if (ret != 0)
+		return ret;
 
 	kfree(nesmr);
 
@@ -2939,6 +3130,9 @@ int nes_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 			nesqp->hwqp.qp_id, attr->qp_state, nesqp->ibqp_state,
 			nesqp->iwarp_state, atomic_read(&nesqp->refcount));
 
+	if (ibqp->qp_type == IB_QPT_RAW_PACKET)
+		return 0;
+
 	spin_lock_irqsave(&nesqp->lock, qplockflags);
 
 	nes_debug(NES_DBG_MOD_QP, "QP%u: hw_iwarp_state=0x%X, hw_tcp_state=0x%X,"
@@ -3208,8 +3402,10 @@ int nes_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
  */
 static int nes_multicast_attach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
 {
-	nes_debug(NES_DBG_INIT, "\n");
-	return -ENOSYS;
+	int ret =  -ENOSYS;
+	struct nes_qp *nesqp = to_nesqp(ibqp);
+	ret =  nes_ud_subscribe_mcast(nesqp->rx_ud_wq, gid);
+	return ret;
 }
 
 
@@ -3218,8 +3414,10 @@ static int nes_multicast_attach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
  */
 static int nes_multicast_detach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
 {
-	nes_debug(NES_DBG_INIT, "\n");
-	return -ENOSYS;
+	int ret =  -ENOSYS;
+	struct nes_qp *nesqp = to_nesqp(ibqp);
+	ret =  nes_ud_unsubscribe_mcast(nesqp->rx_ud_wq, gid);
+	return ret;
 }
 
 
@@ -3846,6 +4044,7 @@ struct nes_ib_device *nes_init_ofa_device(struct net_device *netdev)
 		return NULL;
 	}
 	strlcpy(nesibdev->ibdev.name, "nes%d", IB_DEVICE_NAME_MAX);
+	strcpy(nesibdev->ibdev.name, netdev->name);
 	nesibdev->ibdev.owner = THIS_MODULE;
 
 	nesibdev->ibdev.node_type = RDMA_NODE_RNIC;
@@ -3868,6 +4067,9 @@ struct nes_ib_device *nes_init_ofa_device(struct net_device *netdev)
 			(1ull << IB_USER_VERBS_CMD_REQ_NOTIFY_CQ) |
 			(1ull << IB_USER_VERBS_CMD_CREATE_QP) |
 			(1ull << IB_USER_VERBS_CMD_MODIFY_QP) |
+			(1ull << IB_USER_VERBS_CMD_QUERY_QP) |
+			(1ull << IB_USER_VERBS_CMD_ATTACH_MCAST) |
+			(1ull << IB_USER_VERBS_CMD_DETACH_MCAST) |
 			(1ull << IB_USER_VERBS_CMD_POLL_CQ) |
 			(1ull << IB_USER_VERBS_CMD_DESTROY_QP) |
 			(1ull << IB_USER_VERBS_CMD_ALLOC_MW) |
@@ -3911,8 +4113,9 @@ struct nes_ib_device *nes_init_ofa_device(struct net_device *netdev)
 	nesibdev->ibdev.alloc_fast_reg_page_list = nes_alloc_fast_reg_page_list;
 	nesibdev->ibdev.free_fast_reg_page_list = nes_free_fast_reg_page_list;
 
-	nesibdev->ibdev.attach_mcast = nes_multicast_attach;
 	nesibdev->ibdev.detach_mcast = nes_multicast_detach;
+	nesibdev->ibdev.attach_mcast = nes_multicast_attach;
+
 	nesibdev->ibdev.process_mad = nes_process_mad;
 
 	nesibdev->ibdev.req_notify_cq = nes_req_notify_cq;
diff --git a/drivers/infiniband/hw/nes/nes_verbs.h b/drivers/infiniband/hw/nes/nes_verbs.h
index 2df9993..cbb6585 100644
--- a/drivers/infiniband/hw/nes/nes_verbs.h
+++ b/drivers/infiniband/hw/nes/nes_verbs.h
@@ -79,6 +79,7 @@ struct nes_mr {
 	u16               pbls_used;
 	u8                mode;
 	u8                pbl_4k;
+	u32               mcrqf;
 };
 
 struct nes_hw_pb {
@@ -116,7 +117,8 @@ struct nes_cq {
 	spinlock_t       lock;
 	u8               virtual_cq;
 	u8               pad[3];
-	u32		 mcrqf;
+	atomic_t         usecnt;
+	u32              mcrqf;
 };
 
 struct nes_wq {
@@ -130,6 +132,7 @@ struct disconn_work {
 
 struct iw_cm_id;
 struct ietf_mpa_frame;
+struct nes_ud_file;
 
 struct nes_qp {
 	struct ib_qp          ibqp;
@@ -176,5 +179,7 @@ struct nes_qp {
 	u8                    hw_tcp_state;
 	u8                    term_flags;
 	u8                    sq_kmapped;
+	struct nes_ud_file    *rx_ud_wq;
+	struct nes_ud_file    *tx_ud_wq;
 };
 #endif			/* NES_VERBS_H */


--
To unsubscribe from this list: send the line "unsubscribe linux-rdma" in
the body of a message to majordomo-u79uwXL29TY76Z2rM5mHXA@public.gmane.org
More majordomo info at  http://vger.kernel.org/majordomo-info.html