include cleanup: Update gfp.h and slab.h includes to prepare for breaking implicit...
[safe/jmp/linux-2.6] / drivers / infiniband / hw / nes / nes_hw.c
index 735c125..c36a3f5 100644 (file)
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2006 - 2008 NetEffect, Inc. All rights reserved.
+ * Copyright (c) 2006 - 2009 Intel Corporation.  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
@@ -39,6 +39,7 @@
 #include <linux/tcp.h>
 #include <linux/if_vlan.h>
 #include <linux/inet_lro.h>
+#include <linux/slab.h>
 
 #include "nes.h"
 
@@ -46,6 +47,10 @@ static unsigned int nes_lro_max_aggr = NES_LRO_MAX_AGGR;
 module_param(nes_lro_max_aggr, uint, 0444);
 MODULE_PARM_DESC(nes_lro_max_aggr, "NIC LRO max packet aggregation");
 
+static int wide_ppm_offset;
+module_param(wide_ppm_offset, int, 0644);
+MODULE_PARM_DESC(wide_ppm_offset, "Increase CX4 interface clock ppm offset, 0=100ppm (default), 1=300ppm");
+
 static u32 crit_err_count;
 u32 int_mod_timer_init;
 u32 int_mod_cq_depth_256;
@@ -70,6 +75,8 @@ static void nes_process_iwarp_aeqe(struct nes_device *nesdev,
 static void process_critical_error(struct nes_device *nesdev);
 static void nes_process_mac_intr(struct nes_device *nesdev, u32 mac_number);
 static unsigned int nes_reset_adapter_ne020(struct nes_device *nesdev, u8 *OneG_Mode);
+static void nes_terminate_timeout(unsigned long context);
+static void nes_terminate_start_timer(struct nes_qp *nesqp);
 
 #ifdef CONFIG_INFINIBAND_NES_DEBUG
 static unsigned char *nes_iwarp_state_str[] = {
@@ -254,6 +261,7 @@ struct nes_adapter *nes_init_adapter(struct nes_device *nesdev, u8 hw_rev) {
        u32 adapter_size;
        u32 arp_table_size;
        u16 vendor_id;
+       u16 device_id;
        u8  OneG_Mode;
        u8  func_index;
 
@@ -356,6 +364,13 @@ struct nes_adapter *nes_init_adapter(struct nes_device *nesdev, u8 hw_rev) {
                return NULL;
        }
 
+       nesadapter->vendor_id = (((u32) nesadapter->mac_addr_high) << 8) |
+                               (nesadapter->mac_addr_low >> 24);
+
+       pci_bus_read_config_word(nesdev->pcidev->bus, nesdev->pcidev->devfn,
+                                PCI_DEVICE_ID, &device_id);
+       nesadapter->vendor_part_id = device_id;
+
        if (nes_init_serdes(nesdev, hw_rev, port_count, nesadapter,
                                                        OneG_Mode)) {
                kfree(nesadapter);
@@ -410,8 +425,9 @@ struct nes_adapter *nes_init_adapter(struct nes_device *nesdev, u8 hw_rev) {
 
        nesadapter->base_pd = 1;
 
-       nesadapter->device_cap_flags =
-               IB_DEVICE_LOCAL_DMA_LKEY | IB_DEVICE_MEM_WINDOW;
+       nesadapter->device_cap_flags = IB_DEVICE_LOCAL_DMA_LKEY |
+                                      IB_DEVICE_MEM_WINDOW |
+                                      IB_DEVICE_MEM_MGT_EXTENSIONS;
 
        nesadapter->allocated_qps = (unsigned long *)&(((unsigned char *)nesadapter)
                        [(sizeof(struct nes_adapter)+(sizeof(unsigned long)-1))&(~(sizeof(unsigned long)-1))]);
@@ -422,11 +438,12 @@ struct nes_adapter *nes_init_adapter(struct nes_device *nesdev, u8 hw_rev) {
        nesadapter->qp_table = (struct nes_qp **)(&nesadapter->allocated_arps[BITS_TO_LONGS(arp_table_size)]);
 
 
-       /* mark the usual suspect QPs and CQs as in use */
+       /* mark the usual suspect QPs, MR and CQs as in use */
        for (u32temp = 0; u32temp < NES_FIRST_QPN; u32temp++) {
                set_bit(u32temp, nesadapter->allocated_qps);
                set_bit(u32temp, nesadapter->allocated_cqs);
        }
+       set_bit(0, nesadapter->allocated_mrs);
 
        for (u32temp = 0; u32temp < 20; u32temp++)
                set_bit(u32temp, nesadapter->allocated_pds);
@@ -467,7 +484,7 @@ struct nes_adapter *nes_init_adapter(struct nes_device *nesdev, u8 hw_rev) {
        nesadapter->max_irrq_wr = (u32temp >> 16) & 3;
 
        nesadapter->max_sge = 4;
-       nesadapter->max_cqe = 32767;
+       nesadapter->max_cqe = 32766;
 
        if (nes_read_eeprom_values(nesdev, nesadapter)) {
                printk(KERN_ERR PFX "Unable to read EEPROM data.\n");
@@ -539,7 +556,7 @@ struct nes_adapter *nes_init_adapter(struct nes_device *nesdev, u8 hw_rev) {
                }
                if (int_cnt > 1) {
                        spin_lock_irqsave(&nesadapter->phy_lock, flags);
-                       nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_COMMON_CONTROL1, 0x0000F088);
+                       nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_COMMON_CONTROL1, 0x0000F0C8);
                        mh_detected++;
                        reset_value = nes_read32(nesdev->regs+NES_SOFTWARE_RESET);
                        reset_value |= 0x0000003d;
@@ -564,7 +581,7 @@ struct nes_adapter *nes_init_adapter(struct nes_device *nesdev, u8 hw_rev) {
                                        if (++ext_cnt > int_cnt) {
                                                spin_lock_irqsave(&nesadapter->phy_lock, flags);
                                                nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_COMMON_CONTROL1,
-                                                               0x0000F0C8);
+                                                               0x0000F088);
                                                mh_detected++;
                                                reset_value = nes_read32(nesdev->regs+NES_SOFTWARE_RESET);
                                                reset_value |= 0x0000003d;
@@ -655,7 +672,7 @@ static unsigned int nes_reset_adapter_ne020(struct nes_device *nesdev, u8 *OneG_
                i = 0;
                while (((nes_read32(nesdev->regs+NES_SOFTWARE_RESET) & 0x00000040) == 0) && i++ < 10000)
                        mdelay(1);
-               if (i >= 10000) {
+               if (i > 10000) {
                        nes_debug(NES_DBG_INIT, "Did not see full soft reset done.\n");
                        return 0;
                }
@@ -663,7 +680,7 @@ static unsigned int nes_reset_adapter_ne020(struct nes_device *nesdev, u8 *OneG_
                i = 0;
                while ((nes_read_indexed(nesdev, NES_IDX_INT_CPU_STATUS) != 0x80) && i++ < 10000)
                        mdelay(1);
-               if (i >= 10000) {
+               if (i > 10000) {
                        printk(KERN_ERR PFX "Internal CPU not ready, status = %02X\n",
                               nes_read_indexed(nesdev, NES_IDX_INT_CPU_STATUS));
                        return 0;
@@ -689,7 +706,7 @@ static unsigned int nes_reset_adapter_ne020(struct nes_device *nesdev, u8 *OneG_
        i = 0;
        while (((nes_read32(nesdev->regs+NES_SOFTWARE_RESET) & 0x00000040) == 0) && i++ < 10000)
                mdelay(1);
-       if (i >= 10000) {
+       if (i > 10000) {
                nes_debug(NES_DBG_INIT, "Did not see port soft reset done.\n");
                return 0;
        }
@@ -699,7 +716,7 @@ static unsigned int nes_reset_adapter_ne020(struct nes_device *nesdev, u8 *OneG_
        while (((u32temp = (nes_read_indexed(nesdev, NES_IDX_ETH_SERDES_COMMON_STATUS0)
                        & 0x0000000f)) != 0x0000000f) && i++ < 5000)
                mdelay(1);
-       if (i >= 5000) {
+       if (i > 5000) {
                nes_debug(NES_DBG_INIT, "Serdes 0 not ready, status=%x\n", u32temp);
                return 0;
        }
@@ -710,7 +727,7 @@ static unsigned int nes_reset_adapter_ne020(struct nes_device *nesdev, u8 *OneG_
                while (((u32temp = (nes_read_indexed(nesdev, NES_IDX_ETH_SERDES_COMMON_STATUS1)
                                & 0x0000000f)) != 0x0000000f) && i++ < 5000)
                        mdelay(1);
-               if (i >= 5000) {
+               if (i > 5000) {
                        nes_debug(NES_DBG_INIT, "Serdes 1 not ready, status=%x\n", u32temp);
                        return 0;
                }
@@ -728,38 +745,65 @@ static int nes_init_serdes(struct nes_device *nesdev, u8 hw_rev, u8 port_count,
 {
        int i;
        u32 u32temp;
-       u32 serdes_common_control;
+       u32 sds;
 
        if (hw_rev != NE020_REV) {
                /* init serdes 0 */
+               switch (nesadapter->phy_type[0]) {
+               case NES_PHY_TYPE_CX4:
+                       if (wide_ppm_offset)
+                               nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_CDR_CONTROL0, 0x000FFFAA);
+                       else
+                               nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_CDR_CONTROL0, 0x000000FF);
+                       break;
+               case NES_PHY_TYPE_KR:
+                       nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_CDR_CONTROL0, 0x000000FF);
+                       nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_TX_EMP0, 0x00000000);
+                       break;
+               case NES_PHY_TYPE_PUMA_1G:
+                       nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_CDR_CONTROL0, 0x000000FF);
+                       sds = nes_read_indexed(nesdev, NES_IDX_ETH_SERDES_COMMON_CONTROL0);
+                       sds |= 0x00000100;
+                       nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_COMMON_CONTROL0, sds);
+                       break;
+               default:
+                       nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_CDR_CONTROL0, 0x000000FF);
+                       break;
+               }
 
-               nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_CDR_CONTROL0, 0x000000FF);
-               if (nesadapter->phy_type[0] == NES_PHY_TYPE_PUMA_1G) {
-                       serdes_common_control = nes_read_indexed(nesdev,
-                                       NES_IDX_ETH_SERDES_COMMON_CONTROL0);
-                       serdes_common_control |= 0x000000100;
-                       nes_write_indexed(nesdev,
-                                       NES_IDX_ETH_SERDES_COMMON_CONTROL0,
-                                       serdes_common_control);
-               } else if (!OneG_Mode) {
+               if (!OneG_Mode)
                        nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_TX_HIGHZ_LANE_MODE0, 0x11110000);
-               }
-               if (((port_count > 1) &&
-                       (nesadapter->phy_type[0] != NES_PHY_TYPE_PUMA_1G)) ||
-                       ((port_count > 2) &&
-                       (nesadapter->phy_type[0] == NES_PHY_TYPE_PUMA_1G))) {
-                       /* init serdes 1 */
+
+               if (port_count < 2)
+                       return 0;
+
+               /* init serdes 1 */
+               if (!(OneG_Mode && (nesadapter->phy_type[1] != NES_PHY_TYPE_PUMA_1G)))
                        nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_CDR_CONTROL1, 0x000000FF);
-                       if (nesadapter->phy_type[0] == NES_PHY_TYPE_PUMA_1G) {
-                               serdes_common_control = nes_read_indexed(nesdev,
-                                       NES_IDX_ETH_SERDES_COMMON_CONTROL1);
-                               serdes_common_control |= 0x000000100;
-                               nes_write_indexed(nesdev,
-                                       NES_IDX_ETH_SERDES_COMMON_CONTROL1,
-                                       serdes_common_control);
-                       } else if (!OneG_Mode) {
-                               nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_TX_HIGHZ_LANE_MODE1, 0x11110000);
-                       }
+
+               switch (nesadapter->phy_type[1]) {
+               case NES_PHY_TYPE_ARGUS:
+               case NES_PHY_TYPE_SFP_D:
+                       nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_TX_EMP0, 0x00000000);
+                       nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_TX_EMP1, 0x00000000);
+                       break;
+               case NES_PHY_TYPE_CX4:
+                       if (wide_ppm_offset)
+                               nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_CDR_CONTROL1, 0x000FFFAA);
+                       break;
+               case NES_PHY_TYPE_KR:
+                       nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_TX_EMP1, 0x00000000);
+                       break;
+               case NES_PHY_TYPE_PUMA_1G:
+                       sds = nes_read_indexed(nesdev, NES_IDX_ETH_SERDES_COMMON_CONTROL1);
+                       sds |= 0x000000100;
+                       nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_COMMON_CONTROL1, sds);
+               }
+               if (!OneG_Mode) {
+                       nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_TX_HIGHZ_LANE_MODE1, 0x11110000);
+                       sds = nes_read_indexed(nesdev, NES_IDX_ETH_SERDES_COMMON_CONTROL1);
+                       sds &= 0xFFFFFFBF;
+                       nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_COMMON_CONTROL1, sds);
                }
        } else {
                /* init serdes 0 */
@@ -768,7 +812,7 @@ static int nes_init_serdes(struct nes_device *nesdev, u8 hw_rev, u8 port_count,
                while (((u32temp = (nes_read_indexed(nesdev, NES_IDX_ETH_SERDES_COMMON_STATUS0)
                                & 0x0000000f)) != 0x0000000f) && i++ < 5000)
                        mdelay(1);
-               if (i >= 5000) {
+               if (i > 5000) {
                        nes_debug(NES_DBG_PHY, "Init: serdes 0 not ready, status=%x\n", u32temp);
                        return 1;
                }
@@ -791,7 +835,7 @@ static int nes_init_serdes(struct nes_device *nesdev, u8 hw_rev, u8 port_count,
                        while (((u32temp = (nes_read_indexed(nesdev, NES_IDX_ETH_SERDES_COMMON_STATUS1)
                                & 0x0000000f)) != 0x0000000f) && (i++ < 5000))
                                mdelay(1);
-                       if (i >= 5000) {
+                       if (i > 5000) {
                                printk("%s: Init: serdes 1 not ready, status=%x\n", __func__, u32temp);
                                /* return 1; */
                        }
@@ -888,6 +932,12 @@ static void nes_init_csr_ne020(struct nes_device *nesdev, u8 hw_rev, u8 port_cou
                u32temp &= 0x7fffffff;
                u32temp |= 0x7fff0010;
                nes_write_indexed(nesdev, 0x000021f8, u32temp);
+               if (port_count > 1) {
+                       u32temp = nes_read_indexed(nesdev, 0x000023f8);
+                       u32temp &= 0x7fffffff;
+                       u32temp |= 0x7fff0010;
+                       nes_write_indexed(nesdev, 0x000023f8, u32temp);
+               }
        }
 }
 
@@ -1245,211 +1295,252 @@ int nes_destroy_cqp(struct nes_device *nesdev)
 
 
 /**
- * nes_init_phy
+ * nes_init_1g_phy
  */
-int nes_init_phy(struct nes_device *nesdev)
+int nes_init_1g_phy(struct nes_device *nesdev, u8 phy_type, u8 phy_index)
 {
-       struct nes_adapter *nesadapter = nesdev->nesadapter;
        u32 counter = 0;
-       u32 sds_common_control0;
-       u32 mac_index = nesdev->mac_index;
-       u32 tx_config = 0;
        u16 phy_data;
+       int ret = 0;
+
+       nes_read_1G_phy_reg(nesdev, 1, phy_index, &phy_data);
+       nes_write_1G_phy_reg(nesdev, 23, phy_index, 0xb000);
+
+       /* Reset the PHY */
+       nes_write_1G_phy_reg(nesdev, 0, phy_index, 0x8000);
+       udelay(100);
+       counter = 0;
+       do {
+               nes_read_1G_phy_reg(nesdev, 0, phy_index, &phy_data);
+               if (counter++ > 100) {
+                       ret = -1;
+                       break;
+               }
+       } while (phy_data & 0x8000);
+
+       /* Setting no phy loopback */
+       phy_data &= 0xbfff;
+       phy_data |= 0x1140;
+       nes_write_1G_phy_reg(nesdev, 0, phy_index,  phy_data);
+       nes_read_1G_phy_reg(nesdev, 0, phy_index, &phy_data);
+       nes_read_1G_phy_reg(nesdev, 0x17, phy_index, &phy_data);
+       nes_read_1G_phy_reg(nesdev, 0x1e, phy_index, &phy_data);
+
+       /* Setting the interrupt mask */
+       nes_read_1G_phy_reg(nesdev, 0x19, phy_index, &phy_data);
+       nes_write_1G_phy_reg(nesdev, 0x19, phy_index, 0xffee);
+       nes_read_1G_phy_reg(nesdev, 0x19, phy_index, &phy_data);
+
+       /* turning on flow control */
+       nes_read_1G_phy_reg(nesdev, 4, phy_index, &phy_data);
+       nes_write_1G_phy_reg(nesdev, 4, phy_index, (phy_data & ~(0x03E0)) | 0xc00);
+       nes_read_1G_phy_reg(nesdev, 4, phy_index, &phy_data);
+
+       /* Clear Half duplex */
+       nes_read_1G_phy_reg(nesdev, 9, phy_index, &phy_data);
+       nes_write_1G_phy_reg(nesdev, 9, phy_index, phy_data & ~(0x0100));
+       nes_read_1G_phy_reg(nesdev, 9, phy_index, &phy_data);
+
+       nes_read_1G_phy_reg(nesdev, 0, phy_index, &phy_data);
+       nes_write_1G_phy_reg(nesdev, 0, phy_index, phy_data | 0x0300);
+
+       return ret;
+}
+
+
+/**
+ * nes_init_2025_phy
+ */
+int nes_init_2025_phy(struct nes_device *nesdev, u8 phy_type, u8 phy_index)
+{
        u32 temp_phy_data = 0;
        u32 temp_phy_data2 = 0;
-       u32 i = 0;
-
-       if ((nesadapter->OneG_Mode) &&
-           (nesadapter->phy_type[mac_index] != NES_PHY_TYPE_PUMA_1G)) {
-               nes_debug(NES_DBG_PHY, "1G PHY, mac_index = %d.\n", mac_index);
-               if (nesadapter->phy_type[mac_index] == NES_PHY_TYPE_1G) {
-                       printk(PFX "%s: Programming mdc config for 1G\n", __func__);
-                       tx_config = nes_read_indexed(nesdev, NES_IDX_MAC_TX_CONFIG);
-                       tx_config &= 0xFFFFFFE3;
-                       tx_config |= 0x04;
-                       nes_write_indexed(nesdev, NES_IDX_MAC_TX_CONFIG, tx_config);
+       u32 counter = 0;
+       u32 sds;
+       u32 mac_index = nesdev->mac_index;
+       int ret = 0;
+       unsigned int first_attempt = 1;
+
+       /* Check firmware heartbeat */
+       nes_read_10G_phy_reg(nesdev, phy_index, 0x3, 0xd7ee);
+       temp_phy_data = (u16)nes_read_indexed(nesdev, NES_IDX_MAC_MDIO_CONTROL);
+       udelay(1500);
+       nes_read_10G_phy_reg(nesdev, phy_index, 0x3, 0xd7ee);
+       temp_phy_data2 = (u16)nes_read_indexed(nesdev, NES_IDX_MAC_MDIO_CONTROL);
+
+       if (temp_phy_data != temp_phy_data2) {
+               nes_read_10G_phy_reg(nesdev, phy_index, 0x3, 0xd7fd);
+               temp_phy_data = (u16)nes_read_indexed(nesdev, NES_IDX_MAC_MDIO_CONTROL);
+               if ((temp_phy_data & 0xff) > 0x20)
+                       return 0;
+               printk(PFX "Reinitialize external PHY\n");
+       }
+
+       /* no heartbeat, configure the PHY */
+       nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0x0000, 0x8000);
+       nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xc300, 0x0000);
+       nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xc316, 0x000A);
+       nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xc318, 0x0052);
+
+       switch (phy_type) {
+       case NES_PHY_TYPE_ARGUS:
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xc316, 0x000A);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xc318, 0x0052);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xc302, 0x000C);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xc319, 0x0008);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x3, 0x0027, 0x0001);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xc31a, 0x0098);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x3, 0x0026, 0x0E00);
+
+               /* setup LEDs */
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xd006, 0x0007);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xd007, 0x000A);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xd008, 0x0009);
+               break;
+
+       case NES_PHY_TYPE_SFP_D:
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xc316, 0x000A);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xc318, 0x0052);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xc302, 0x0004);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xc319, 0x0038);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x3, 0x0027, 0x0013);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xc31a, 0x0098);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x3, 0x0026, 0x0E00);
+
+               /* setup LEDs */
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xd006, 0x0007);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xd007, 0x000A);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xd008, 0x0009);
+               break;
+
+       case NES_PHY_TYPE_KR:
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xc316, 0x000A);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xc318, 0x0052);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xc302, 0x000C);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xc319, 0x0010);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x3, 0x0027, 0x0013);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xc31a, 0x0080);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x3, 0x0026, 0x0E00);
+
+               /* setup LEDs */
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xd006, 0x000B);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xd007, 0x0003);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xd008, 0x0004);
+
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x3, 0x0022, 0x406D);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x3, 0x0023, 0x0020);
+               break;
+       }
+
+       nes_write_10G_phy_reg(nesdev, phy_index, 0x3, 0x0028, 0xA528);
+
+       /* Bring PHY out of reset */
+       nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xc300, 0x0002);
+
+       /* Check for heartbeat */
+       counter = 0;
+       mdelay(690);
+       nes_read_10G_phy_reg(nesdev, phy_index, 0x3, 0xd7ee);
+       temp_phy_data = (u16)nes_read_indexed(nesdev, NES_IDX_MAC_MDIO_CONTROL);
+       do {
+               if (counter++ > 150) {
+                       printk(PFX "No PHY heartbeat\n");
+                       break;
                }
+               mdelay(1);
+               nes_read_10G_phy_reg(nesdev, phy_index, 0x3, 0xd7ee);
+               temp_phy_data2 = (u16)nes_read_indexed(nesdev, NES_IDX_MAC_MDIO_CONTROL);
+       } while ((temp_phy_data2 == temp_phy_data));
 
-               nes_read_1G_phy_reg(nesdev, 1, nesadapter->phy_index[mac_index], &phy_data);
-               nes_debug(NES_DBG_PHY, "Phy data from register 1 phy address %u = 0x%X.\n",
-                               nesadapter->phy_index[mac_index], phy_data);
-               nes_write_1G_phy_reg(nesdev, 23, nesadapter->phy_index[mac_index], 0xb000);
-
-               /* Reset the PHY */
-               nes_write_1G_phy_reg(nesdev, 0, nesadapter->phy_index[mac_index], 0x8000);
-               udelay(100);
-               counter = 0;
-               do {
-                       nes_read_1G_phy_reg(nesdev, 0, nesadapter->phy_index[mac_index], &phy_data);
-                       nes_debug(NES_DBG_PHY, "Phy data from register 0 = 0x%X.\n", phy_data);
-                       if (counter++ > 100) break;
-               } while (phy_data & 0x8000);
-
-               /* Setting no phy loopback */
-               phy_data &= 0xbfff;
-               phy_data |= 0x1140;
-               nes_write_1G_phy_reg(nesdev, 0, nesadapter->phy_index[mac_index],  phy_data);
-               nes_read_1G_phy_reg(nesdev, 0, nesadapter->phy_index[mac_index], &phy_data);
-               nes_debug(NES_DBG_PHY, "Phy data from register 0 = 0x%X.\n", phy_data);
-
-               nes_read_1G_phy_reg(nesdev, 0x17, nesadapter->phy_index[mac_index], &phy_data);
-               nes_debug(NES_DBG_PHY, "Phy data from register 0x17 = 0x%X.\n", phy_data);
-
-               nes_read_1G_phy_reg(nesdev, 0x1e, nesadapter->phy_index[mac_index], &phy_data);
-               nes_debug(NES_DBG_PHY, "Phy data from register 0x1e = 0x%X.\n", phy_data);
-
-               /* Setting the interrupt mask */
-               nes_read_1G_phy_reg(nesdev, 0x19, nesadapter->phy_index[mac_index], &phy_data);
-               nes_debug(NES_DBG_PHY, "Phy data from register 0x19 = 0x%X.\n", phy_data);
-               nes_write_1G_phy_reg(nesdev, 0x19, nesadapter->phy_index[mac_index], 0xffee);
-
-               nes_read_1G_phy_reg(nesdev, 0x19, nesadapter->phy_index[mac_index], &phy_data);
-               nes_debug(NES_DBG_PHY, "Phy data from register 0x19 = 0x%X.\n", phy_data);
-
-               /* turning on flow control */
-               nes_read_1G_phy_reg(nesdev, 4, nesadapter->phy_index[mac_index], &phy_data);
-               nes_debug(NES_DBG_PHY, "Phy data from register 0x4 = 0x%X.\n", phy_data);
-               nes_write_1G_phy_reg(nesdev, 4, nesadapter->phy_index[mac_index],
-                               (phy_data & ~(0x03E0)) | 0xc00);
-               /* nes_write_1G_phy_reg(nesdev, 4, nesadapter->phy_index[mac_index],
-                               phy_data | 0xc00); */
-               nes_read_1G_phy_reg(nesdev, 4, nesadapter->phy_index[mac_index], &phy_data);
-               nes_debug(NES_DBG_PHY, "Phy data from register 0x4 = 0x%X.\n", phy_data);
-
-               nes_read_1G_phy_reg(nesdev, 9, nesadapter->phy_index[mac_index], &phy_data);
-               nes_debug(NES_DBG_PHY, "Phy data from register 0x9 = 0x%X.\n", phy_data);
-               /* Clear Half duplex */
-               nes_write_1G_phy_reg(nesdev, 9, nesadapter->phy_index[mac_index],
-                               phy_data & ~(0x0100));
-               nes_read_1G_phy_reg(nesdev, 9, nesadapter->phy_index[mac_index], &phy_data);
-               nes_debug(NES_DBG_PHY, "Phy data from register 0x9 = 0x%X.\n", phy_data);
-
-               nes_read_1G_phy_reg(nesdev, 0, nesadapter->phy_index[mac_index], &phy_data);
-               nes_write_1G_phy_reg(nesdev, 0, nesadapter->phy_index[mac_index], phy_data | 0x0300);
-       } else {
-               if ((nesadapter->phy_type[mac_index] == NES_PHY_TYPE_IRIS) ||
-                   (nesadapter->phy_type[mac_index] == NES_PHY_TYPE_ARGUS)) {
-                       /* setup 10G MDIO operation */
-                       tx_config = nes_read_indexed(nesdev, NES_IDX_MAC_TX_CONFIG);
-                       tx_config &= 0xFFFFFFE3;
-                       tx_config |= 0x15;
-                       nes_write_indexed(nesdev, NES_IDX_MAC_TX_CONFIG, tx_config);
+       /* wait for tracking */
+       counter = 0;
+       do {
+               nes_read_10G_phy_reg(nesdev, phy_index, 0x3, 0xd7fd);
+               temp_phy_data = (u16)nes_read_indexed(nesdev, NES_IDX_MAC_MDIO_CONTROL);
+               if (counter++ > 300) {
+                       if (((temp_phy_data & 0xff) == 0x0) && first_attempt) {
+                               first_attempt = 0;
+                               counter = 0;
+                               /* reset AMCC PHY and try again */
+                               nes_write_10G_phy_reg(nesdev, phy_index, 0x3, 0xe854, 0x00c0);
+                               nes_write_10G_phy_reg(nesdev, phy_index, 0x3, 0xe854, 0x0040);
+                               continue;
+                       } else {
+                               ret = 1;
+                               break;
+                       }
                }
-               if ((nesadapter->phy_type[mac_index] == NES_PHY_TYPE_ARGUS)) {
-                       nes_read_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 0x3, 0xd7ee);
-
-                       temp_phy_data = (u16)nes_read_indexed(nesdev, NES_IDX_MAC_MDIO_CONTROL);
-                       mdelay(10);
-                       nes_read_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 0x3, 0xd7ee);
-                       temp_phy_data2 = (u16)nes_read_indexed(nesdev, NES_IDX_MAC_MDIO_CONTROL);
-
-                       /*
-                        * if firmware is already running (like from a
-                        * driver un-load/load, don't do anything.
-                        */
-                       if (temp_phy_data == temp_phy_data2) {
-                               /* configure QT2505 AMCC PHY */
-                               nes_write_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 0x1, 0x0000, 0x8000);
-                               nes_write_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 0x1, 0xc300, 0x0000);
-                               nes_write_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 0x1, 0xc302, 0x0044);
-                               nes_write_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 0x1, 0xc318, 0x0052);
-                               nes_write_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 0x1, 0xc319, 0x0008);
-                               nes_write_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 0x1, 0xc31a, 0x0098);
-                               nes_write_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 0x3, 0x0026, 0x0E00);
-                               nes_write_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 0x3, 0x0027, 0x0001);
-                               nes_write_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 0x3, 0x0028, 0xA528);
+               mdelay(10);
+       } while ((temp_phy_data & 0xff) < 0x30);
+
+       /* setup signal integrity */
+       nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xd003, 0x0000);
+       nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xF00D, 0x00FE);
+       nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xF00E, 0x0032);
+       if (phy_type == NES_PHY_TYPE_KR) {
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xF00F, 0x000C);
+       } else {
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xF00F, 0x0002);
+               nes_write_10G_phy_reg(nesdev, phy_index, 0x1, 0xc314, 0x0063);
+       }
 
-                               /*
-                                * remove micro from reset; chip boots from ROM,
-                                * uploads EEPROM f/w image, uC executes f/w
-                                */
-                               nes_write_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 0x1, 0xc300, 0x0002);
+       /* reset serdes */
+       sds = nes_read_indexed(nesdev, NES_IDX_ETH_SERDES_COMMON_CONTROL0 + mac_index * 0x200);
+       sds |= 0x1;
+       nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_COMMON_CONTROL0 + mac_index * 0x200, sds);
+       sds &= 0xfffffffe;
+       nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_COMMON_CONTROL0 + mac_index * 0x200, sds);
 
-                               /*
-                                * wait for heart beat to start to
-                                * know loading is done
-                                */
-                               counter = 0;
-                               do {
-                                       nes_read_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 0x3, 0xd7ee);
-                                       temp_phy_data = (u16)nes_read_indexed(nesdev, NES_IDX_MAC_MDIO_CONTROL);
-                                       if (counter++ > 1000) {
-                                               nes_debug(NES_DBG_PHY, "AMCC PHY- breaking from heartbeat check <this is bad!!!> \n");
-                                               break;
-                                       }
-                                       mdelay(100);
-                                       nes_read_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 0x3, 0xd7ee);
-                                       temp_phy_data2 = (u16)nes_read_indexed(nesdev, NES_IDX_MAC_MDIO_CONTROL);
-                               } while ((temp_phy_data2 == temp_phy_data));
+       counter = 0;
+       while (((nes_read32(nesdev->regs + NES_SOFTWARE_RESET) & 0x00000040) != 0x00000040)
+                       && (counter++ < 5000))
+               ;
 
-                               /*
-                                * wait for tracking to start to know
-                                * f/w is good to go
-                                */
-                               counter = 0;
-                               do {
-                                       nes_read_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 0x3, 0xd7fd);
-                                       temp_phy_data = (u16)nes_read_indexed(nesdev, NES_IDX_MAC_MDIO_CONTROL);
-                                       if (counter++ > 1000) {
-                                               nes_debug(NES_DBG_PHY, "AMCC PHY- breaking from status check <this is bad!!!> \n");
-                                               break;
-                                       }
-                                       mdelay(1000);
-                                       /*
-                                        * nes_debug(NES_DBG_PHY, "AMCC PHY- phy_status not ready yet = 0x%02X\n",
-                                        *                      temp_phy_data);
-                                        */
-                               } while (((temp_phy_data & 0xff) != 0x50) && ((temp_phy_data & 0xff) != 0x70));
-
-                               /* set LOS Control invert RXLOSB_I_PADINV */
-                               nes_write_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 0x1, 0xd003, 0x0000);
-                               /* set LOS Control to mask of RXLOSB_I */
-                               nes_write_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 0x1, 0xc314, 0x0042);
-                               /* set LED1 to input mode (LED1 and LED2 share same LED) */
-                               nes_write_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 0x1, 0xd006, 0x0007);
-                               /* set LED2 to RX link_status and activity */
-                               nes_write_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 0x1, 0xd007, 0x000A);
-                               /* set LED3 to RX link_status */
-                               nes_write_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 0x1, 0xd008, 0x0009);
+       return ret;
+}
 
-                               /*
-                                * reset the res-calibration on t2
-                                * serdes; ensures it is stable after
-                                * the amcc phy is stable
-                                */
 
-                               sds_common_control0  = nes_read_indexed(nesdev, NES_IDX_ETH_SERDES_COMMON_CONTROL0);
-                               sds_common_control0 |= 0x1;
-                               nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_COMMON_CONTROL0, sds_common_control0);
+/**
+ * nes_init_phy
+ */
+int nes_init_phy(struct nes_device *nesdev)
+{
+       struct nes_adapter *nesadapter = nesdev->nesadapter;
+       u32 mac_index = nesdev->mac_index;
+       u32 tx_config = 0;
+       unsigned long flags;
+       u8  phy_type = nesadapter->phy_type[mac_index];
+       u8  phy_index = nesadapter->phy_index[mac_index];
+       int ret = 0;
 
-                               /* release the res-calibration reset */
-                               sds_common_control0 &= 0xfffffffe;
-                               nes_write_indexed(nesdev, NES_IDX_ETH_SERDES_COMMON_CONTROL0, sds_common_control0);
+       tx_config = nes_read_indexed(nesdev, NES_IDX_MAC_TX_CONFIG);
+       if (phy_type == NES_PHY_TYPE_1G) {
+               /* setup 1G MDIO operation */
+               tx_config &= 0xFFFFFFE3;
+               tx_config |= 0x04;
+       } else {
+               /* setup 10G MDIO operation */
+               tx_config &= 0xFFFFFFE3;
+               tx_config |= 0x15;
+       }
+       nes_write_indexed(nesdev, NES_IDX_MAC_TX_CONFIG, tx_config);
 
-                               i = 0;
-                               while (((nes_read32(nesdev->regs + NES_SOFTWARE_RESET) & 0x00000040) != 0x00000040)
-                                               && (i++ < 5000)) {
-                                       /* mdelay(1); */
-                               }
+       spin_lock_irqsave(&nesdev->nesadapter->phy_lock, flags);
 
-                               /*
-                                * wait for link train done before moving on,
-                                * or will get an interupt storm
-                                */
-                               counter = 0;
-                               do {
-                                       temp_phy_data = nes_read_indexed(nesdev, NES_IDX_PHY_PCS_CONTROL_STATUS0 +
-                                                               (0x200 * (nesdev->mac_index & 1)));
-                                       if (counter++ > 1000) {
-                                               nes_debug(NES_DBG_PHY, "AMCC PHY- breaking from link train wait <this is bad, link didnt train!!!>\n");
-                                               break;
-                                       }
-                                       mdelay(1);
-                               } while (((temp_phy_data & 0x0f1f0000) != 0x0f0f0000));
-                       }
-               }
+       switch (phy_type) {
+       case NES_PHY_TYPE_1G:
+               ret = nes_init_1g_phy(nesdev, phy_type, phy_index);
+               break;
+       case NES_PHY_TYPE_ARGUS:
+       case NES_PHY_TYPE_SFP_D:
+       case NES_PHY_TYPE_KR:
+               ret = nes_init_2025_phy(nesdev, phy_type, phy_index);
+               break;
        }
-       return 0;
+
+       spin_unlock_irqrestore(&nesdev->nesadapter->phy_lock, flags);
+
+       return ret;
 }
 
 
@@ -1636,7 +1727,6 @@ int nes_init_nic_qp(struct nes_device *nesdev, struct net_device *netdev)
        nesvnic->post_cqp_request = nes_post_cqp_request;
        nesvnic->mcrq_mcast_filter = NULL;
 
-       spin_lock_init(&nesvnic->nic.sq_lock);
        spin_lock_init(&nesvnic->nic.rq_lock);
 
        /* setup the RQ */
@@ -1810,9 +1900,14 @@ void nes_destroy_nic_qp(struct nes_vnic *nesvnic)
        u16  wqe_fragment_index;
        u64 wqe_frag;
        u32 cqp_head;
+       u32 wqm_cfg0;
        unsigned long flags;
        int ret;
 
+       /* clear wqe stall before destroying NIC QP */
+       wqm_cfg0 = nes_read_indexed(nesdev, NES_IDX_WQM_CONFIG0);
+       nes_write_indexed(nesdev, NES_IDX_WQM_CONFIG0, wqm_cfg0 & 0xFFFF7FFF);
+
        /* Free remaining NIC receive buffers */
        while (nesvnic->nic.rq_head != nesvnic->nic.rq_tail) {
                nic_rqe   = &nesvnic->nic.rq_vbase[nesvnic->nic.rq_tail];
@@ -1931,6 +2026,9 @@ void nes_destroy_nic_qp(struct nes_vnic *nesvnic)
 
        pci_free_consistent(nesdev->pcidev, nesvnic->nic_mem_size, nesvnic->nic_vbase,
                        nesvnic->nic_pbase);
+
+       /* restore old wqm_cfg0 value */
+       nes_write_indexed(nesdev, NES_IDX_WQM_CONFIG0, wqm_cfg0);
 }
 
 /**
@@ -2261,6 +2359,8 @@ static void nes_process_aeq(struct nes_device *nesdev, struct nes_hw_aeq *aeq)
 
                if (++head >= aeq_size)
                        head = 0;
+
+               nes_write32(nesdev->regs + NES_AEQ_ALLOC, 1 << 16);
        }
        while (1);
        aeq->aeq_head = head;
@@ -2350,6 +2450,7 @@ static void nes_process_mac_intr(struct nes_device *nesdev, u32 mac_number)
        u16 temp_phy_data;
        u32 pcs_val  = 0x0f0f0000;
        u32 pcs_mask = 0x0f1f0000;
+       u32 cdr_ctrl;
 
        spin_lock_irqsave(&nesadapter->phy_lock, flags);
        if (nesadapter->mac_sw_state[mac_number] != NES_MAC_SW_IDLE) {
@@ -2448,22 +2549,9 @@ static void nes_process_mac_intr(struct nes_device *nesdev, u32 mac_number)
                        }
                } else {
                        switch (nesadapter->phy_type[mac_index]) {
-                       case NES_PHY_TYPE_IRIS:
-                               nes_read_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 1, 1);
-                               temp_phy_data = (u16)nes_read_indexed(nesdev, NES_IDX_MAC_MDIO_CONTROL);
-                               u32temp = 20;
-                               do {
-                                       nes_read_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 1, 1);
-                                       phy_data = (u16)nes_read_indexed(nesdev, NES_IDX_MAC_MDIO_CONTROL);
-                                       if ((phy_data == temp_phy_data) || (!(--u32temp)))
-                                               break;
-                                       temp_phy_data = phy_data;
-                               } while (1);
-                               nes_debug(NES_DBG_PHY, "%s: Phy data = 0x%04X, link was %s.\n",
-                                       __func__, phy_data, nesadapter->mac_link_down[mac_index] ? "DOWN" : "UP");
-                               break;
-
                        case NES_PHY_TYPE_ARGUS:
+                       case NES_PHY_TYPE_SFP_D:
+                       case NES_PHY_TYPE_KR:
                                /* clear the alarms */
                                nes_read_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 4, 0x0008);
                                nes_read_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 4, 0xc001);
@@ -2474,19 +2562,18 @@ static void nes_process_mac_intr(struct nes_device *nesdev, u32 mac_number)
                                nes_read_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 1, 0x9004);
                                nes_read_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 1, 0x9005);
                                /* check link status */
-                               nes_read_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 1, 1);
+                               nes_read_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 1, 0x9003);
                                temp_phy_data = (u16)nes_read_indexed(nesdev, NES_IDX_MAC_MDIO_CONTROL);
-                               u32temp = 100;
-                               do {
-                                       nes_read_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 1, 1);
 
-                                       phy_data = (u16)nes_read_indexed(nesdev, NES_IDX_MAC_MDIO_CONTROL);
-                                       if ((phy_data == temp_phy_data) || (!(--u32temp)))
-                                               break;
-                                       temp_phy_data = phy_data;
-                               } while (1);
+                               nes_read_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 3, 0x0021);
+                               nes_read_indexed(nesdev, NES_IDX_MAC_MDIO_CONTROL);
+                               nes_read_10G_phy_reg(nesdev, nesadapter->phy_index[mac_index], 3, 0x0021);
+                               phy_data = (u16)nes_read_indexed(nesdev, NES_IDX_MAC_MDIO_CONTROL);
+
+                               phy_data = (!temp_phy_data && (phy_data == 0x8000)) ? 0x4 : 0x0;
+
                                nes_debug(NES_DBG_PHY, "%s: Phy data = 0x%04X, link was %s.\n",
-                                       __func__, phy_data, nesadapter->mac_link_down ? "DOWN" : "UP");
+                                       __func__, phy_data, nesadapter->mac_link_down[mac_index] ? "DOWN" : "UP");
                                break;
 
                        case NES_PHY_TYPE_PUMA_1G:
@@ -2502,6 +2589,17 @@ static void nes_process_mac_intr(struct nes_device *nesdev, u32 mac_number)
                }
 
                if (phy_data & 0x0004) {
+                       if (wide_ppm_offset &&
+                           (nesadapter->phy_type[mac_index] == NES_PHY_TYPE_CX4) &&
+                           (nesadapter->hw_rev != NE020_REV)) {
+                               cdr_ctrl = nes_read_indexed(nesdev,
+                                                           NES_IDX_ETH_SERDES_CDR_CONTROL0 +
+                                                           mac_index * 0x200);
+                               nes_write_indexed(nesdev,
+                                                 NES_IDX_ETH_SERDES_CDR_CONTROL0 +
+                                                 mac_index * 0x200,
+                                                 cdr_ctrl | 0x000F0000);
+                       }
                        nesadapter->mac_link_down[mac_index] = 0;
                        list_for_each_entry(nesvnic, &nesadapter->nesvnic_list[mac_index], list) {
                                nes_debug(NES_DBG_PHY, "The Link is UP!!.  linkup was %d\n",
@@ -2516,6 +2614,17 @@ static void nes_process_mac_intr(struct nes_device *nesdev, u32 mac_number)
                                }
                        }
                } else {
+                       if (wide_ppm_offset &&
+                           (nesadapter->phy_type[mac_index] == NES_PHY_TYPE_CX4) &&
+                           (nesadapter->hw_rev != NE020_REV)) {
+                               cdr_ctrl = nes_read_indexed(nesdev,
+                                                           NES_IDX_ETH_SERDES_CDR_CONTROL0 +
+                                                           mac_index * 0x200);
+                               nes_write_indexed(nesdev,
+                                                 NES_IDX_ETH_SERDES_CDR_CONTROL0 +
+                                                 mac_index * 0x200,
+                                                 cdr_ctrl & 0xFFF0FFFF);
+                       }
                        nesadapter->mac_link_down[mac_index] = 1;
                        list_for_each_entry(nesvnic, &nesadapter->nesvnic_list[mac_index], list) {
                                nes_debug(NES_DBG_PHY, "The Link is Down!!. linkup was %d\n",
@@ -2541,7 +2650,7 @@ static void nes_nic_napi_ce_handler(struct nes_device *nesdev, struct nes_hw_nic
 {
        struct nes_vnic *nesvnic = container_of(cq, struct nes_vnic, nic_cq);
 
-       netif_rx_schedule(&nesvnic->napi);
+       napi_schedule(&nesvnic->napi);
 }
 
 
@@ -2622,9 +2731,9 @@ void nes_nic_ce_handler(struct nes_device *nesdev, struct nes_hw_nic_cq *cq)
                                                } else
                                                        break;
                                        }
-                                       if (skb)
-                                               dev_kfree_skb_any(skb);
                                }
+                               if (skb)
+                                       dev_kfree_skb_any(skb);
                                nesnic->sq_tail++;
                                nesnic->sq_tail &= nesnic->sq_size-1;
                                if (sq_cqes > 128) {
@@ -2700,28 +2809,34 @@ void nes_nic_ce_handler(struct nes_device *nesdev, struct nes_hw_nic_cq *cq)
                                                        pkt_type, (pkt_type & NES_PKT_TYPE_APBVT_MASK)); */
 
                                if ((pkt_type & NES_PKT_TYPE_APBVT_MASK) == NES_PKT_TYPE_APBVT_BITS) {
-                                       nes_cm_recv(rx_skb, nesvnic->netdev);
+                                       if (nes_cm_recv(rx_skb, nesvnic->netdev))
+                                               rx_skb = NULL;
+                               }
+                               if (rx_skb == NULL)
+                                       goto skip_rx_indicate0;
+
+
+                               if ((cqe_misc & NES_NIC_CQE_TAG_VALID) &&
+                                   (nesvnic->vlan_grp != NULL)) {
+                                       vlan_tag = (u16)(le32_to_cpu(
+                                                       cq->cq_vbase[head].cqe_words[NES_NIC_CQE_TAG_PKT_TYPE_IDX])
+                                                       >> 16);
+                                       nes_debug(NES_DBG_CQ, "%s: Reporting stripped VLAN packet. Tag = 0x%04X\n",
+                                                       nesvnic->netdev->name, vlan_tag);
+                                       if (nes_use_lro)
+                                               lro_vlan_hwaccel_receive_skb(&nesvnic->lro_mgr, rx_skb,
+                                                               nesvnic->vlan_grp, vlan_tag, NULL);
+                                       else
+                                               nes_vlan_rx(rx_skb, nesvnic->vlan_grp, vlan_tag);
                                } else {
-                                       if ((cqe_misc & NES_NIC_CQE_TAG_VALID) && (nesvnic->vlan_grp != NULL)) {
-                                               vlan_tag = (u16)(le32_to_cpu(
-                                                               cq->cq_vbase[head].cqe_words[NES_NIC_CQE_TAG_PKT_TYPE_IDX])
-                                                               >> 16);
-                                               nes_debug(NES_DBG_CQ, "%s: Reporting stripped VLAN packet. Tag = 0x%04X\n",
-                                                               nesvnic->netdev->name, vlan_tag);
-                                               if (nes_use_lro)
-                                                       lro_vlan_hwaccel_receive_skb(&nesvnic->lro_mgr, rx_skb,
-                                                                       nesvnic->vlan_grp, vlan_tag, NULL);
-                                               else
-                                                       nes_vlan_rx(rx_skb, nesvnic->vlan_grp, vlan_tag);
-                                       } else {
-                                               if (nes_use_lro)
-                                                       lro_receive_skb(&nesvnic->lro_mgr, rx_skb, NULL);
-                                               else
-                                                       nes_netif_rx(rx_skb);
-                                       }
+                                       if (nes_use_lro)
+                                               lro_receive_skb(&nesvnic->lro_mgr, rx_skb, NULL);
+                                       else
+                                               nes_netif_rx(rx_skb);
                                }
 
-                               nesvnic->netdev->last_rx = jiffies;
+skip_rx_indicate0:
+                               ;
                                /* nesvnic->netstats.rx_packets++; */
                                /* nesvnic->netstats.rx_bytes += rx_pkt_size; */
                        }
@@ -2883,6 +2998,417 @@ static void nes_cqp_ce_handler(struct nes_device *nesdev, struct nes_hw_cq *cq)
 }
 
 
+static u8 *locate_mpa(u8 *pkt, u32 aeq_info)
+{
+       u16 pkt_len;
+
+       if (aeq_info & NES_AEQE_Q2_DATA_ETHERNET) {
+               /* skip over ethernet header */
+               pkt_len = be16_to_cpu(*(u16 *)(pkt + ETH_HLEN - 2));
+               pkt += ETH_HLEN;
+
+               /* Skip over IP and TCP headers */
+               pkt += 4 * (pkt[0] & 0x0f);
+               pkt += 4 * ((pkt[12] >> 4) & 0x0f);
+       }
+       return pkt;
+}
+
+/* Determine if incoming error pkt is rdma layer */
+static u32 iwarp_opcode(struct nes_qp *nesqp, u32 aeq_info)
+{
+       u8 *pkt;
+       u16 *mpa;
+       u32 opcode = 0xffffffff;
+
+       if (aeq_info & NES_AEQE_Q2_DATA_WRITTEN) {
+               pkt = nesqp->hwqp.q2_vbase + BAD_FRAME_OFFSET;
+               mpa = (u16 *)locate_mpa(pkt, aeq_info);
+               opcode = be16_to_cpu(mpa[1]) & 0xf;
+       }
+
+       return opcode;
+}
+
+/* Build iWARP terminate header */
+static int nes_bld_terminate_hdr(struct nes_qp *nesqp, u16 async_event_id, u32 aeq_info)
+{
+       u8 *pkt = nesqp->hwqp.q2_vbase + BAD_FRAME_OFFSET;
+       u16 ddp_seg_len;
+       int copy_len = 0;
+       u8 is_tagged = 0;
+       u8 flush_code = 0;
+       struct nes_terminate_hdr *termhdr;
+
+       termhdr = (struct nes_terminate_hdr *)nesqp->hwqp.q2_vbase;
+       memset(termhdr, 0, 64);
+
+       if (aeq_info & NES_AEQE_Q2_DATA_WRITTEN) {
+
+               /* Use data from offending packet to fill in ddp & rdma hdrs */
+               pkt = locate_mpa(pkt, aeq_info);
+               ddp_seg_len = be16_to_cpu(*(u16 *)pkt);
+               if (ddp_seg_len) {
+                       copy_len = 2;
+                       termhdr->hdrct = DDP_LEN_FLAG;
+                       if (pkt[2] & 0x80) {
+                               is_tagged = 1;
+                               if (ddp_seg_len >= TERM_DDP_LEN_TAGGED) {
+                                       copy_len += TERM_DDP_LEN_TAGGED;
+                                       termhdr->hdrct |= DDP_HDR_FLAG;
+                               }
+                       } else {
+                               if (ddp_seg_len >= TERM_DDP_LEN_UNTAGGED) {
+                                       copy_len += TERM_DDP_LEN_UNTAGGED;
+                                       termhdr->hdrct |= DDP_HDR_FLAG;
+                               }
+
+                               if (ddp_seg_len >= (TERM_DDP_LEN_UNTAGGED + TERM_RDMA_LEN)) {
+                                       if ((pkt[3] & RDMA_OPCODE_MASK) == RDMA_READ_REQ_OPCODE) {
+                                               copy_len += TERM_RDMA_LEN;
+                                               termhdr->hdrct |= RDMA_HDR_FLAG;
+                                       }
+                               }
+                       }
+               }
+       }
+
+       switch (async_event_id) {
+       case NES_AEQE_AEID_AMP_UNALLOCATED_STAG:
+               switch (iwarp_opcode(nesqp, aeq_info)) {
+               case IWARP_OPCODE_WRITE:
+                       flush_code = IB_WC_LOC_PROT_ERR;
+                       termhdr->layer_etype = (LAYER_DDP << 4) | DDP_TAGGED_BUFFER;
+                       termhdr->error_code = DDP_TAGGED_INV_STAG;
+                       break;
+               default:
+                       flush_code = IB_WC_REM_ACCESS_ERR;
+                       termhdr->layer_etype = (LAYER_RDMA << 4) | RDMAP_REMOTE_PROT;
+                       termhdr->error_code = RDMAP_INV_STAG;
+               }
+               break;
+       case NES_AEQE_AEID_AMP_INVALID_STAG:
+               flush_code = IB_WC_REM_ACCESS_ERR;
+               termhdr->layer_etype = (LAYER_RDMA << 4) | RDMAP_REMOTE_PROT;
+               termhdr->error_code = RDMAP_INV_STAG;
+               break;
+       case NES_AEQE_AEID_AMP_BAD_QP:
+               flush_code = IB_WC_LOC_QP_OP_ERR;
+               termhdr->layer_etype = (LAYER_DDP << 4) | DDP_UNTAGGED_BUFFER;
+               termhdr->error_code = DDP_UNTAGGED_INV_QN;
+               break;
+       case NES_AEQE_AEID_AMP_BAD_STAG_KEY:
+       case NES_AEQE_AEID_AMP_BAD_STAG_INDEX:
+               switch (iwarp_opcode(nesqp, aeq_info)) {
+               case IWARP_OPCODE_SEND_INV:
+               case IWARP_OPCODE_SEND_SE_INV:
+                       flush_code = IB_WC_REM_OP_ERR;
+                       termhdr->layer_etype = (LAYER_RDMA << 4) | RDMAP_REMOTE_OP;
+                       termhdr->error_code = RDMAP_CANT_INV_STAG;
+                       break;
+               default:
+                       flush_code = IB_WC_REM_ACCESS_ERR;
+                       termhdr->layer_etype = (LAYER_RDMA << 4) | RDMAP_REMOTE_PROT;
+                       termhdr->error_code = RDMAP_INV_STAG;
+               }
+               break;
+       case NES_AEQE_AEID_AMP_BOUNDS_VIOLATION:
+               if (aeq_info & (NES_AEQE_Q2_DATA_ETHERNET | NES_AEQE_Q2_DATA_MPA)) {
+                       flush_code = IB_WC_LOC_PROT_ERR;
+                       termhdr->layer_etype = (LAYER_DDP << 4) | DDP_TAGGED_BUFFER;
+                       termhdr->error_code = DDP_TAGGED_BOUNDS;
+               } else {
+                       flush_code = IB_WC_REM_ACCESS_ERR;
+                       termhdr->layer_etype = (LAYER_RDMA << 4) | RDMAP_REMOTE_PROT;
+                       termhdr->error_code = RDMAP_INV_BOUNDS;
+               }
+               break;
+       case NES_AEQE_AEID_AMP_RIGHTS_VIOLATION:
+       case NES_AEQE_AEID_AMP_INVALIDATE_NO_REMOTE_ACCESS_RIGHTS:
+       case NES_AEQE_AEID_PRIV_OPERATION_DENIED:
+               flush_code = IB_WC_REM_ACCESS_ERR;
+               termhdr->layer_etype = (LAYER_RDMA << 4) | RDMAP_REMOTE_PROT;
+               termhdr->error_code = RDMAP_ACCESS;
+               break;
+       case NES_AEQE_AEID_AMP_TO_WRAP:
+               flush_code = IB_WC_REM_ACCESS_ERR;
+               termhdr->layer_etype = (LAYER_RDMA << 4) | RDMAP_REMOTE_PROT;
+               termhdr->error_code = RDMAP_TO_WRAP;
+               break;
+       case NES_AEQE_AEID_AMP_BAD_PD:
+               switch (iwarp_opcode(nesqp, aeq_info)) {
+               case IWARP_OPCODE_WRITE:
+                       flush_code = IB_WC_LOC_PROT_ERR;
+                       termhdr->layer_etype = (LAYER_DDP << 4) | DDP_TAGGED_BUFFER;
+                       termhdr->error_code = DDP_TAGGED_UNASSOC_STAG;
+                       break;
+               case IWARP_OPCODE_SEND_INV:
+               case IWARP_OPCODE_SEND_SE_INV:
+                       flush_code = IB_WC_REM_ACCESS_ERR;
+                       termhdr->layer_etype = (LAYER_RDMA << 4) | RDMAP_REMOTE_PROT;
+                       termhdr->error_code = RDMAP_CANT_INV_STAG;
+                       break;
+               default:
+                       flush_code = IB_WC_REM_ACCESS_ERR;
+                       termhdr->layer_etype = (LAYER_RDMA << 4) | RDMAP_REMOTE_PROT;
+                       termhdr->error_code = RDMAP_UNASSOC_STAG;
+               }
+               break;
+       case NES_AEQE_AEID_LLP_RECEIVED_MARKER_AND_LENGTH_FIELDS_DONT_MATCH:
+               flush_code = IB_WC_LOC_LEN_ERR;
+               termhdr->layer_etype = (LAYER_MPA << 4) | DDP_LLP;
+               termhdr->error_code = MPA_MARKER;
+               break;
+       case NES_AEQE_AEID_LLP_RECEIVED_MPA_CRC_ERROR:
+               flush_code = IB_WC_GENERAL_ERR;
+               termhdr->layer_etype = (LAYER_MPA << 4) | DDP_LLP;
+               termhdr->error_code = MPA_CRC;
+               break;
+       case NES_AEQE_AEID_LLP_SEGMENT_TOO_LARGE:
+       case NES_AEQE_AEID_LLP_SEGMENT_TOO_SMALL:
+               flush_code = IB_WC_LOC_LEN_ERR;
+               termhdr->layer_etype = (LAYER_DDP << 4) | DDP_CATASTROPHIC;
+               termhdr->error_code = DDP_CATASTROPHIC_LOCAL;
+               break;
+       case NES_AEQE_AEID_DDP_LCE_LOCAL_CATASTROPHIC:
+       case NES_AEQE_AEID_DDP_NO_L_BIT:
+               flush_code = IB_WC_FATAL_ERR;
+               termhdr->layer_etype = (LAYER_DDP << 4) | DDP_CATASTROPHIC;
+               termhdr->error_code = DDP_CATASTROPHIC_LOCAL;
+               break;
+       case NES_AEQE_AEID_DDP_INVALID_MSN_GAP_IN_MSN:
+       case NES_AEQE_AEID_DDP_INVALID_MSN_RANGE_IS_NOT_VALID:
+               flush_code = IB_WC_GENERAL_ERR;
+               termhdr->layer_etype = (LAYER_DDP << 4) | DDP_UNTAGGED_BUFFER;
+               termhdr->error_code = DDP_UNTAGGED_INV_MSN_RANGE;
+               break;
+       case NES_AEQE_AEID_DDP_UBE_DDP_MESSAGE_TOO_LONG_FOR_AVAILABLE_BUFFER:
+               flush_code = IB_WC_LOC_LEN_ERR;
+               termhdr->layer_etype = (LAYER_DDP << 4) | DDP_UNTAGGED_BUFFER;
+               termhdr->error_code = DDP_UNTAGGED_INV_TOO_LONG;
+               break;
+       case NES_AEQE_AEID_DDP_UBE_INVALID_DDP_VERSION:
+               flush_code = IB_WC_GENERAL_ERR;
+               if (is_tagged) {
+                       termhdr->layer_etype = (LAYER_DDP << 4) | DDP_TAGGED_BUFFER;
+                       termhdr->error_code = DDP_TAGGED_INV_DDP_VER;
+               } else {
+                       termhdr->layer_etype = (LAYER_DDP << 4) | DDP_UNTAGGED_BUFFER;
+                       termhdr->error_code = DDP_UNTAGGED_INV_DDP_VER;
+               }
+               break;
+       case NES_AEQE_AEID_DDP_UBE_INVALID_MO:
+               flush_code = IB_WC_GENERAL_ERR;
+               termhdr->layer_etype = (LAYER_DDP << 4) | DDP_UNTAGGED_BUFFER;
+               termhdr->error_code = DDP_UNTAGGED_INV_MO;
+               break;
+       case NES_AEQE_AEID_DDP_UBE_INVALID_MSN_NO_BUFFER_AVAILABLE:
+               flush_code = IB_WC_REM_OP_ERR;
+               termhdr->layer_etype = (LAYER_DDP << 4) | DDP_UNTAGGED_BUFFER;
+               termhdr->error_code = DDP_UNTAGGED_INV_MSN_NO_BUF;
+               break;
+       case NES_AEQE_AEID_DDP_UBE_INVALID_QN:
+               flush_code = IB_WC_GENERAL_ERR;
+               termhdr->layer_etype = (LAYER_DDP << 4) | DDP_UNTAGGED_BUFFER;
+               termhdr->error_code = DDP_UNTAGGED_INV_QN;
+               break;
+       case NES_AEQE_AEID_RDMAP_ROE_INVALID_RDMAP_VERSION:
+               flush_code = IB_WC_GENERAL_ERR;
+               termhdr->layer_etype = (LAYER_RDMA << 4) | RDMAP_REMOTE_OP;
+               termhdr->error_code = RDMAP_INV_RDMAP_VER;
+               break;
+       case NES_AEQE_AEID_RDMAP_ROE_UNEXPECTED_OPCODE:
+               flush_code = IB_WC_LOC_QP_OP_ERR;
+               termhdr->layer_etype = (LAYER_RDMA << 4) | RDMAP_REMOTE_OP;
+               termhdr->error_code = RDMAP_UNEXPECTED_OP;
+               break;
+       default:
+               flush_code = IB_WC_FATAL_ERR;
+               termhdr->layer_etype = (LAYER_RDMA << 4) | RDMAP_REMOTE_OP;
+               termhdr->error_code = RDMAP_UNSPECIFIED;
+               break;
+       }
+
+       if (copy_len)
+               memcpy(termhdr + 1, pkt, copy_len);
+
+       if ((flush_code) && ((NES_AEQE_INBOUND_RDMA & aeq_info) == 0)) {
+               if (aeq_info & NES_AEQE_SQ)
+                       nesqp->term_sq_flush_code = flush_code;
+               else
+                       nesqp->term_rq_flush_code = flush_code;
+       }
+
+       return sizeof(struct nes_terminate_hdr) + copy_len;
+}
+
+static void nes_terminate_connection(struct nes_device *nesdev, struct nes_qp *nesqp,
+                struct nes_hw_aeqe *aeqe, enum ib_event_type eventtype)
+{
+       u64 context;
+       unsigned long flags;
+       u32 aeq_info;
+       u16 async_event_id;
+       u8 tcp_state;
+       u8 iwarp_state;
+       u32 termlen = 0;
+       u32 mod_qp_flags = NES_CQP_QP_IWARP_STATE_TERMINATE |
+                          NES_CQP_QP_TERM_DONT_SEND_FIN;
+       struct nes_adapter *nesadapter = nesdev->nesadapter;
+
+       if (nesqp->term_flags & NES_TERM_SENT)
+               return; /* Sanity check */
+
+       aeq_info = le32_to_cpu(aeqe->aeqe_words[NES_AEQE_MISC_IDX]);
+       tcp_state = (aeq_info & NES_AEQE_TCP_STATE_MASK) >> NES_AEQE_TCP_STATE_SHIFT;
+       iwarp_state = (aeq_info & NES_AEQE_IWARP_STATE_MASK) >> NES_AEQE_IWARP_STATE_SHIFT;
+       async_event_id = (u16)aeq_info;
+
+       context = (unsigned long)nesadapter->qp_table[le32_to_cpu(
+               aeqe->aeqe_words[NES_AEQE_COMP_QP_CQ_ID_IDX]) - NES_FIRST_QPN];
+       if (!context) {
+               WARN_ON(!context);
+               return;
+       }
+
+       nesqp = (struct nes_qp *)(unsigned long)context;
+       spin_lock_irqsave(&nesqp->lock, flags);
+       nesqp->hw_iwarp_state = iwarp_state;
+       nesqp->hw_tcp_state = tcp_state;
+       nesqp->last_aeq = async_event_id;
+       nesqp->terminate_eventtype = eventtype;
+       spin_unlock_irqrestore(&nesqp->lock, flags);
+
+       if (nesadapter->send_term_ok)
+               termlen = nes_bld_terminate_hdr(nesqp, async_event_id, aeq_info);
+       else
+               mod_qp_flags |= NES_CQP_QP_TERM_DONT_SEND_TERM_MSG;
+
+       nes_terminate_start_timer(nesqp);
+       nesqp->term_flags |= NES_TERM_SENT;
+       nes_hw_modify_qp(nesdev, nesqp, mod_qp_flags, termlen, 0);
+}
+
+static void nes_terminate_send_fin(struct nes_device *nesdev,
+                         struct nes_qp *nesqp, struct nes_hw_aeqe *aeqe)
+{
+       u32 aeq_info;
+       u16 async_event_id;
+       u8 tcp_state;
+       u8 iwarp_state;
+       unsigned long flags;
+
+       aeq_info = le32_to_cpu(aeqe->aeqe_words[NES_AEQE_MISC_IDX]);
+       tcp_state = (aeq_info & NES_AEQE_TCP_STATE_MASK) >> NES_AEQE_TCP_STATE_SHIFT;
+       iwarp_state = (aeq_info & NES_AEQE_IWARP_STATE_MASK) >> NES_AEQE_IWARP_STATE_SHIFT;
+       async_event_id = (u16)aeq_info;
+
+       spin_lock_irqsave(&nesqp->lock, flags);
+       nesqp->hw_iwarp_state = iwarp_state;
+       nesqp->hw_tcp_state = tcp_state;
+       nesqp->last_aeq = async_event_id;
+       spin_unlock_irqrestore(&nesqp->lock, flags);
+
+       /* Send the fin only */
+       nes_hw_modify_qp(nesdev, nesqp, NES_CQP_QP_IWARP_STATE_TERMINATE |
+               NES_CQP_QP_TERM_DONT_SEND_TERM_MSG, 0, 0);
+}
+
+/* Cleanup after a terminate sent or received */
+static void nes_terminate_done(struct nes_qp *nesqp, int timeout_occurred)
+{
+       u32 next_iwarp_state = NES_CQP_QP_IWARP_STATE_ERROR;
+       unsigned long flags;
+       struct nes_vnic *nesvnic = to_nesvnic(nesqp->ibqp.device);
+       struct nes_device *nesdev = nesvnic->nesdev;
+       u8 first_time = 0;
+
+       spin_lock_irqsave(&nesqp->lock, flags);
+       if (nesqp->hte_added) {
+               nesqp->hte_added = 0;
+               next_iwarp_state |= NES_CQP_QP_DEL_HTE;
+       }
+
+       first_time = (nesqp->term_flags & NES_TERM_DONE) == 0;
+       nesqp->term_flags |= NES_TERM_DONE;
+       spin_unlock_irqrestore(&nesqp->lock, flags);
+
+       /* Make sure we go through this only once */
+       if (first_time) {
+               if (timeout_occurred == 0)
+                       del_timer(&nesqp->terminate_timer);
+               else
+                       next_iwarp_state |= NES_CQP_QP_RESET;
+
+               nes_hw_modify_qp(nesdev, nesqp, next_iwarp_state, 0, 0);
+               nes_cm_disconn(nesqp);
+       }
+}
+
+static void nes_terminate_received(struct nes_device *nesdev,
+                               struct nes_qp *nesqp, struct nes_hw_aeqe *aeqe)
+{
+       u32 aeq_info;
+       u8 *pkt;
+       u32 *mpa;
+       u8 ddp_ctl;
+       u8 rdma_ctl;
+       u16 aeq_id = 0;
+
+       aeq_info = le32_to_cpu(aeqe->aeqe_words[NES_AEQE_MISC_IDX]);
+       if (aeq_info & NES_AEQE_Q2_DATA_WRITTEN) {
+               /* Terminate is not a performance path so the silicon */
+               /* did not validate the frame - do it now */
+               pkt = nesqp->hwqp.q2_vbase + BAD_FRAME_OFFSET;
+               mpa = (u32 *)locate_mpa(pkt, aeq_info);
+               ddp_ctl = (be32_to_cpu(mpa[0]) >> 8) & 0xff;
+               rdma_ctl = be32_to_cpu(mpa[0]) & 0xff;
+               if ((ddp_ctl & 0xc0) != 0x40)
+                       aeq_id = NES_AEQE_AEID_DDP_LCE_LOCAL_CATASTROPHIC;
+               else if ((ddp_ctl & 0x03) != 1)
+                       aeq_id = NES_AEQE_AEID_DDP_UBE_INVALID_DDP_VERSION;
+               else if (be32_to_cpu(mpa[2]) != 2)
+                       aeq_id = NES_AEQE_AEID_DDP_UBE_INVALID_QN;
+               else if (be32_to_cpu(mpa[3]) != 1)
+                       aeq_id = NES_AEQE_AEID_DDP_INVALID_MSN_GAP_IN_MSN;
+               else if (be32_to_cpu(mpa[4]) != 0)
+                       aeq_id = NES_AEQE_AEID_DDP_UBE_INVALID_MO;
+               else if ((rdma_ctl & 0xc0) != 0x40)
+                       aeq_id = NES_AEQE_AEID_RDMAP_ROE_INVALID_RDMAP_VERSION;
+
+               if (aeq_id) {
+                       /* Bad terminate recvd - send back a terminate */
+                       aeq_info = (aeq_info & 0xffff0000) | aeq_id;
+                       aeqe->aeqe_words[NES_AEQE_MISC_IDX] = cpu_to_le32(aeq_info);
+                       nes_terminate_connection(nesdev, nesqp, aeqe, IB_EVENT_QP_FATAL);
+                       return;
+               }
+       }
+
+       nesqp->term_flags |= NES_TERM_RCVD;
+       nesqp->terminate_eventtype = IB_EVENT_QP_FATAL;
+       nes_terminate_start_timer(nesqp);
+       nes_terminate_send_fin(nesdev, nesqp, aeqe);
+}
+
+/* Timeout routine in case terminate fails to complete */
+static void nes_terminate_timeout(unsigned long context)
+{
+       struct nes_qp *nesqp = (struct nes_qp *)(unsigned long)context;
+
+       nes_terminate_done(nesqp, 1);
+}
+
+/* Set a timer in case hw cannot complete the terminate sequence */
+static void nes_terminate_start_timer(struct nes_qp *nesqp)
+{
+       init_timer(&nesqp->terminate_timer);
+       nesqp->terminate_timer.function = nes_terminate_timeout;
+       nesqp->terminate_timer.expires = jiffies + HZ;
+       nesqp->terminate_timer.data = (unsigned long)nesqp;
+       add_timer(&nesqp->terminate_timer);
+}
+
 /**
  * nes_process_iwarp_aeqe
  */
@@ -2890,33 +3416,32 @@ static void nes_process_iwarp_aeqe(struct nes_device *nesdev,
                                   struct nes_hw_aeqe *aeqe)
 {
        u64 context;
-       u64 aeqe_context = 0;
        unsigned long flags;
        struct nes_qp *nesqp;
+       struct nes_hw_cq *hw_cq;
+       struct nes_cq *nescq;
        int resource_allocated;
-       /* struct iw_cm_id *cm_id; */
        struct nes_adapter *nesadapter = nesdev->nesadapter;
-       struct ib_event ibevent;
-       /* struct iw_cm_event cm_event; */
        u32 aeq_info;
        u32 next_iwarp_state = 0;
        u16 async_event_id;
        u8 tcp_state;
        u8 iwarp_state;
+       struct ib_event ibevent;
 
        nes_debug(NES_DBG_AEQ, "\n");
        aeq_info = le32_to_cpu(aeqe->aeqe_words[NES_AEQE_MISC_IDX]);
-       if ((NES_AEQE_INBOUND_RDMA&aeq_info) || (!(NES_AEQE_QP&aeq_info))) {
+       if ((NES_AEQE_INBOUND_RDMA & aeq_info) || (!(NES_AEQE_QP & aeq_info))) {
                context  = le32_to_cpu(aeqe->aeqe_words[NES_AEQE_COMP_CTXT_LOW_IDX]);
                context += ((u64)le32_to_cpu(aeqe->aeqe_words[NES_AEQE_COMP_CTXT_HIGH_IDX])) << 32;
        } else {
-               aeqe_context = le32_to_cpu(aeqe->aeqe_words[NES_AEQE_COMP_CTXT_LOW_IDX]);
-               aeqe_context += ((u64)le32_to_cpu(aeqe->aeqe_words[NES_AEQE_COMP_CTXT_HIGH_IDX])) << 32;
                context = (unsigned long)nesadapter->qp_table[le32_to_cpu(
                                                aeqe->aeqe_words[NES_AEQE_COMP_QP_CQ_ID_IDX]) - NES_FIRST_QPN];
                BUG_ON(!context);
        }
 
+       /* context is nesqp unless async_event_id == CQ ERROR */
+       nesqp = (struct nes_qp *)(unsigned long)context;
        async_event_id = (u16)aeq_info;
        tcp_state = (aeq_info & NES_AEQE_TCP_STATE_MASK) >> NES_AEQE_TCP_STATE_SHIFT;
        iwarp_state = (aeq_info & NES_AEQE_IWARP_STATE_MASK) >> NES_AEQE_IWARP_STATE_SHIFT;
@@ -2928,7 +3453,9 @@ static void nes_process_iwarp_aeqe(struct nes_device *nesdev,
 
        switch (async_event_id) {
                case NES_AEQE_AEID_LLP_FIN_RECEIVED:
-                       nesqp = *((struct nes_qp **)&context);
+                       if (nesqp->term_flags)
+                               return; /* Ignore it, wait for close complete */
+
                        if (atomic_inc_return(&nesqp->close_timer_started) == 1) {
                                nesqp->cm_id->add_ref(nesqp->cm_id);
                                schedule_nes_timer(nesqp->cm_node, (struct sk_buff *)nesqp,
@@ -2939,191 +3466,117 @@ static void nes_process_iwarp_aeqe(struct nes_device *nesdev,
                                                nesqp->hwqp.qp_id, atomic_read(&nesqp->refcount),
                                                async_event_id, nesqp->last_aeq, tcp_state);
                        }
-                       if ((tcp_state != NES_AEQE_TCP_STATE_CLOSE_WAIT) ||
-                                       (nesqp->ibqp_state != IB_QPS_RTS)) {
-                               /* FIN Received but tcp state or IB state moved on,
-                                               should expect a close complete */
-                               return;
-                       }
-               case NES_AEQE_AEID_LLP_CLOSE_COMPLETE:
-               case NES_AEQE_AEID_LLP_CONNECTION_RESET:
-               case NES_AEQE_AEID_TERMINATE_SENT:
-               case NES_AEQE_AEID_RDMAP_ROE_BAD_LLP_CLOSE:
-               case NES_AEQE_AEID_RESET_SENT:
-                       nesqp = *((struct nes_qp **)&context);
-                       if (async_event_id == NES_AEQE_AEID_RESET_SENT) {
-                               tcp_state = NES_AEQE_TCP_STATE_CLOSED;
-                       }
-                       spin_lock_irqsave(&nesqp->lock, flags);
-                       nesqp->hw_iwarp_state = iwarp_state;
-                       nesqp->hw_tcp_state = tcp_state;
-                       nesqp->last_aeq = async_event_id;
-
-                       if ((tcp_state == NES_AEQE_TCP_STATE_CLOSED) ||
-                                       (tcp_state == NES_AEQE_TCP_STATE_TIME_WAIT)) {
-                               nesqp->hte_added = 0;
-                               spin_unlock_irqrestore(&nesqp->lock, flags);
-                               nes_debug(NES_DBG_AEQ, "issuing hw modifyqp for QP%u to remove hte\n",
-                                               nesqp->hwqp.qp_id);
-                               nes_hw_modify_qp(nesdev, nesqp,
-                                               NES_CQP_QP_IWARP_STATE_ERROR | NES_CQP_QP_DEL_HTE, 0);
-                               spin_lock_irqsave(&nesqp->lock, flags);
-                       }
 
-                       if ((nesqp->ibqp_state == IB_QPS_RTS) &&
-                                       ((tcp_state == NES_AEQE_TCP_STATE_CLOSE_WAIT) ||
-                                       (async_event_id == NES_AEQE_AEID_LLP_CONNECTION_RESET))) {
-                               switch (nesqp->hw_iwarp_state) {
-                                       case NES_AEQE_IWARP_STATE_RTS:
-                                               next_iwarp_state = NES_CQP_QP_IWARP_STATE_CLOSING;
-                                               nesqp->hw_iwarp_state = NES_AEQE_IWARP_STATE_CLOSING;
-                                               break;
-                                       case NES_AEQE_IWARP_STATE_TERMINATE:
-                                               next_iwarp_state = NES_CQP_QP_IWARP_STATE_TERMINATE;
-                                               nesqp->hw_iwarp_state = NES_AEQE_IWARP_STATE_TERMINATE;
-                                               if (async_event_id == NES_AEQE_AEID_RDMAP_ROE_BAD_LLP_CLOSE) {
-                                                       next_iwarp_state |= 0x02000000;
-                                                       nesqp->hw_tcp_state = NES_AEQE_TCP_STATE_CLOSED;
-                                               }
-                                               break;
-                                       default:
-                                               next_iwarp_state = 0;
-                               }
-                               spin_unlock_irqrestore(&nesqp->lock, flags);
-                               if (next_iwarp_state) {
-                                       nes_debug(NES_DBG_AEQ, "issuing hw modifyqp for QP%u. next state = 0x%08X,"
-                                                       " also added another reference\n",
-                                                       nesqp->hwqp.qp_id, next_iwarp_state);
-                                       nes_hw_modify_qp(nesdev, nesqp, next_iwarp_state, 0);
-                               }
-                               nes_cm_disconn(nesqp);
-                       } else {
-                               if (async_event_id ==  NES_AEQE_AEID_LLP_FIN_RECEIVED) {
-                                       /* FIN Received but ib state not RTS,
-                                                       close complete will be on its way */
-                                       spin_unlock_irqrestore(&nesqp->lock, flags);
-                                       return;
-                               }
-                               spin_unlock_irqrestore(&nesqp->lock, flags);
-                               if (async_event_id == NES_AEQE_AEID_RDMAP_ROE_BAD_LLP_CLOSE) {
-                                       next_iwarp_state = NES_CQP_QP_IWARP_STATE_TERMINATE | 0x02000000;
-                                       nesqp->hw_tcp_state = NES_AEQE_TCP_STATE_CLOSED;
-                                       nes_debug(NES_DBG_AEQ, "issuing hw modifyqp for QP%u. next state = 0x%08X,"
-                                                       " also added another reference\n",
-                                                       nesqp->hwqp.qp_id, next_iwarp_state);
-                                       nes_hw_modify_qp(nesdev, nesqp, next_iwarp_state, 0);
-                               }
-                               nes_cm_disconn(nesqp);
-                       }
                        break;
-               case NES_AEQE_AEID_LLP_TERMINATE_RECEIVED:
-                       nesqp = *((struct nes_qp **)&context);
-                       spin_lock_irqsave(&nesqp->lock, flags);
-                       nesqp->hw_iwarp_state = iwarp_state;
-                       nesqp->hw_tcp_state = tcp_state;
-                       nesqp->last_aeq = async_event_id;
-                       spin_unlock_irqrestore(&nesqp->lock, flags);
-                       nes_debug(NES_DBG_AEQ, "Processing an NES_AEQE_AEID_LLP_TERMINATE_RECEIVED"
-                                       " event on QP%u \n  Q2 Data:\n",
-                                       nesqp->hwqp.qp_id);
-                       if (nesqp->ibqp.event_handler) {
-                               ibevent.device = nesqp->ibqp.device;
-                               ibevent.element.qp = &nesqp->ibqp;
-                               ibevent.event = IB_EVENT_QP_FATAL;
-                               nesqp->ibqp.event_handler(&ibevent, nesqp->ibqp.qp_context);
-                       }
-                       if ((tcp_state == NES_AEQE_TCP_STATE_CLOSE_WAIT) ||
-                                       ((nesqp->ibqp_state == IB_QPS_RTS)&&
-                                       (async_event_id == NES_AEQE_AEID_LLP_CONNECTION_RESET))) {
-                               nes_cm_disconn(nesqp);
-                       } else {
-                               nesqp->in_disconnect = 0;
-                               wake_up(&nesqp->kick_waitq);
+               case NES_AEQE_AEID_LLP_CLOSE_COMPLETE:
+                       if (nesqp->term_flags) {
+                               nes_terminate_done(nesqp, 0);
+                               return;
                        }
-                       break;
-               case NES_AEQE_AEID_LLP_TOO_MANY_RETRIES:
-                       nesqp = *((struct nes_qp **)&context);
                        spin_lock_irqsave(&nesqp->lock, flags);
-                       nesqp->hw_iwarp_state = NES_AEQE_IWARP_STATE_ERROR;
-                       nesqp->hw_tcp_state = NES_AEQE_TCP_STATE_CLOSED;
-                       nesqp->last_aeq = async_event_id;
-                       if (nesqp->cm_id) {
-                               nes_debug(NES_DBG_AEQ, "Processing an NES_AEQE_AEID_LLP_TOO_MANY_RETRIES"
-                                               " event on QP%u, remote IP = 0x%08X \n",
-                                               nesqp->hwqp.qp_id,
-                                               ntohl(nesqp->cm_id->remote_addr.sin_addr.s_addr));
-                       } else {
-                               nes_debug(NES_DBG_AEQ, "Processing an NES_AEQE_AEID_LLP_TOO_MANY_RETRIES"
-                                               " event on QP%u \n",
-                                               nesqp->hwqp.qp_id);
-                       }
+                       nesqp->hw_iwarp_state = NES_AEQE_IWARP_STATE_CLOSING;
                        spin_unlock_irqrestore(&nesqp->lock, flags);
-                       next_iwarp_state = NES_CQP_QP_IWARP_STATE_ERROR | NES_CQP_QP_RESET;
-                       nes_hw_modify_qp(nesdev, nesqp, next_iwarp_state, 0);
-                       if (nesqp->ibqp.event_handler) {
-                               ibevent.device = nesqp->ibqp.device;
-                               ibevent.element.qp = &nesqp->ibqp;
-                               ibevent.event = IB_EVENT_QP_FATAL;
-                               nesqp->ibqp.event_handler(&ibevent, nesqp->ibqp.qp_context);
-                       }
+                       nes_hw_modify_qp(nesdev, nesqp, NES_CQP_QP_IWARP_STATE_CLOSING, 0, 0);
+                       nes_cm_disconn(nesqp);
                        break;
-               case NES_AEQE_AEID_AMP_BAD_STAG_INDEX:
-                       if (NES_AEQE_INBOUND_RDMA&aeq_info) {
-                               nesqp = nesadapter->qp_table[le32_to_cpu(
-                                               aeqe->aeqe_words[NES_AEQE_COMP_QP_CQ_ID_IDX])-NES_FIRST_QPN];
-                       } else {
-                               /* TODO: get the actual WQE and mask off wqe index */
-                               context &= ~((u64)511);
-                               nesqp = *((struct nes_qp **)&context);
-                       }
+
+               case NES_AEQE_AEID_RESET_SENT:
+                       tcp_state = NES_AEQE_TCP_STATE_CLOSED;
                        spin_lock_irqsave(&nesqp->lock, flags);
                        nesqp->hw_iwarp_state = iwarp_state;
                        nesqp->hw_tcp_state = tcp_state;
                        nesqp->last_aeq = async_event_id;
+                       nesqp->hte_added = 0;
                        spin_unlock_irqrestore(&nesqp->lock, flags);
-                       nes_debug(NES_DBG_AEQ, "Processing an NES_AEQE_AEID_AMP_BAD_STAG_INDEX event on QP%u\n",
-                                       nesqp->hwqp.qp_id);
-                       if (nesqp->ibqp.event_handler) {
-                               ibevent.device = nesqp->ibqp.device;
-                               ibevent.element.qp = &nesqp->ibqp;
-                               ibevent.event = IB_EVENT_QP_ACCESS_ERR;
-                               nesqp->ibqp.event_handler(&ibevent, nesqp->ibqp.qp_context);
-                       }
+                       next_iwarp_state = NES_CQP_QP_IWARP_STATE_ERROR | NES_CQP_QP_DEL_HTE;
+                       nes_hw_modify_qp(nesdev, nesqp, next_iwarp_state, 0, 0);
+                       nes_cm_disconn(nesqp);
                        break;
-               case NES_AEQE_AEID_AMP_UNALLOCATED_STAG:
-                       nesqp = *((struct nes_qp **)&context);
+
+               case NES_AEQE_AEID_LLP_CONNECTION_RESET:
+                       if (atomic_read(&nesqp->close_timer_started))
+                               return;
                        spin_lock_irqsave(&nesqp->lock, flags);
                        nesqp->hw_iwarp_state = iwarp_state;
                        nesqp->hw_tcp_state = tcp_state;
                        nesqp->last_aeq = async_event_id;
                        spin_unlock_irqrestore(&nesqp->lock, flags);
-                       nes_debug(NES_DBG_AEQ, "Processing an NES_AEQE_AEID_AMP_UNALLOCATED_STAG event on QP%u\n",
-                                       nesqp->hwqp.qp_id);
-                       if (nesqp->ibqp.event_handler) {
-                               ibevent.device = nesqp->ibqp.device;
-                               ibevent.element.qp = &nesqp->ibqp;
-                               ibevent.event = IB_EVENT_QP_ACCESS_ERR;
-                               nesqp->ibqp.event_handler(&ibevent, nesqp->ibqp.qp_context);
-                       }
+                       nes_cm_disconn(nesqp);
+                       break;
+
+               case NES_AEQE_AEID_TERMINATE_SENT:
+                       nes_terminate_send_fin(nesdev, nesqp, aeqe);
+                       break;
+
+               case NES_AEQE_AEID_LLP_TERMINATE_RECEIVED:
+                       nes_terminate_received(nesdev, nesqp, aeqe);
                        break;
+
+               case NES_AEQE_AEID_AMP_BAD_STAG_KEY:
+               case NES_AEQE_AEID_AMP_BAD_STAG_INDEX:
+               case NES_AEQE_AEID_AMP_UNALLOCATED_STAG:
+               case NES_AEQE_AEID_AMP_INVALID_STAG:
+               case NES_AEQE_AEID_AMP_RIGHTS_VIOLATION:
+               case NES_AEQE_AEID_AMP_INVALIDATE_NO_REMOTE_ACCESS_RIGHTS:
                case NES_AEQE_AEID_PRIV_OPERATION_DENIED:
-                       nesqp = nesadapter->qp_table[le32_to_cpu(aeqe->aeqe_words
-                                       [NES_AEQE_COMP_QP_CQ_ID_IDX])-NES_FIRST_QPN];
-                       spin_lock_irqsave(&nesqp->lock, flags);
-                       nesqp->hw_iwarp_state = iwarp_state;
-                       nesqp->hw_tcp_state = tcp_state;
-                       nesqp->last_aeq = async_event_id;
-                       spin_unlock_irqrestore(&nesqp->lock, flags);
-                       nes_debug(NES_DBG_AEQ, "Processing an NES_AEQE_AEID_PRIV_OPERATION_DENIED event on QP%u,"
-                                       " nesqp = %p, AE reported %p\n",
-                                       nesqp->hwqp.qp_id, nesqp, *((struct nes_qp **)&context));
-                       if (nesqp->ibqp.event_handler) {
-                               ibevent.device = nesqp->ibqp.device;
-                               ibevent.element.qp = &nesqp->ibqp;
-                               ibevent.event = IB_EVENT_QP_ACCESS_ERR;
-                               nesqp->ibqp.event_handler(&ibevent, nesqp->ibqp.qp_context);
+               case NES_AEQE_AEID_DDP_UBE_DDP_MESSAGE_TOO_LONG_FOR_AVAILABLE_BUFFER:
+               case NES_AEQE_AEID_AMP_BOUNDS_VIOLATION:
+               case NES_AEQE_AEID_AMP_TO_WRAP:
+                       printk(KERN_ERR PFX "QP[%u] async_event_id=0x%04X IB_EVENT_QP_ACCESS_ERR\n",
+                                       nesqp->hwqp.qp_id, async_event_id);
+                       nes_terminate_connection(nesdev, nesqp, aeqe, IB_EVENT_QP_ACCESS_ERR);
+                       break;
+
+               case NES_AEQE_AEID_LLP_SEGMENT_TOO_LARGE:
+               case NES_AEQE_AEID_LLP_SEGMENT_TOO_SMALL:
+               case NES_AEQE_AEID_DDP_UBE_INVALID_MO:
+               case NES_AEQE_AEID_DDP_UBE_INVALID_QN:
+                       if (iwarp_opcode(nesqp, aeq_info) > IWARP_OPCODE_TERM) {
+                               aeq_info &= 0xffff0000;
+                               aeq_info |= NES_AEQE_AEID_RDMAP_ROE_UNEXPECTED_OPCODE;
+                               aeqe->aeqe_words[NES_AEQE_MISC_IDX] = cpu_to_le32(aeq_info);
                        }
+
+               case NES_AEQE_AEID_RDMAP_ROE_BAD_LLP_CLOSE:
+               case NES_AEQE_AEID_LLP_TOO_MANY_RETRIES:
+               case NES_AEQE_AEID_DDP_UBE_INVALID_MSN_NO_BUFFER_AVAILABLE:
+               case NES_AEQE_AEID_LLP_RECEIVED_MPA_CRC_ERROR:
+               case NES_AEQE_AEID_AMP_BAD_QP:
+               case NES_AEQE_AEID_LLP_RECEIVED_MARKER_AND_LENGTH_FIELDS_DONT_MATCH:
+               case NES_AEQE_AEID_DDP_LCE_LOCAL_CATASTROPHIC:
+               case NES_AEQE_AEID_DDP_NO_L_BIT:
+               case NES_AEQE_AEID_DDP_INVALID_MSN_GAP_IN_MSN:
+               case NES_AEQE_AEID_DDP_INVALID_MSN_RANGE_IS_NOT_VALID:
+               case NES_AEQE_AEID_DDP_UBE_INVALID_DDP_VERSION:
+               case NES_AEQE_AEID_RDMAP_ROE_INVALID_RDMAP_VERSION:
+               case NES_AEQE_AEID_RDMAP_ROE_UNEXPECTED_OPCODE:
+               case NES_AEQE_AEID_AMP_BAD_PD:
+               case NES_AEQE_AEID_AMP_FASTREG_SHARED:
+               case NES_AEQE_AEID_AMP_FASTREG_VALID_STAG:
+               case NES_AEQE_AEID_AMP_FASTREG_MW_STAG:
+               case NES_AEQE_AEID_AMP_FASTREG_INVALID_RIGHTS:
+               case NES_AEQE_AEID_AMP_FASTREG_PBL_TABLE_OVERFLOW:
+               case NES_AEQE_AEID_AMP_FASTREG_INVALID_LENGTH:
+               case NES_AEQE_AEID_AMP_INVALIDATE_SHARED:
+               case NES_AEQE_AEID_AMP_INVALIDATE_MR_WITH_BOUND_WINDOWS:
+               case NES_AEQE_AEID_AMP_MWBIND_VALID_STAG:
+               case NES_AEQE_AEID_AMP_MWBIND_OF_MR_STAG:
+               case NES_AEQE_AEID_AMP_MWBIND_TO_ZERO_BASED_STAG:
+               case NES_AEQE_AEID_AMP_MWBIND_TO_MW_STAG:
+               case NES_AEQE_AEID_AMP_MWBIND_INVALID_RIGHTS:
+               case NES_AEQE_AEID_AMP_MWBIND_INVALID_BOUNDS:
+               case NES_AEQE_AEID_AMP_MWBIND_TO_INVALID_PARENT:
+               case NES_AEQE_AEID_AMP_MWBIND_BIND_DISABLED:
+               case NES_AEQE_AEID_BAD_CLOSE:
+               case NES_AEQE_AEID_RDMA_READ_WHILE_ORD_ZERO:
+               case NES_AEQE_AEID_STAG_ZERO_INVALID:
+               case NES_AEQE_AEID_ROE_INVALID_RDMA_READ_REQUEST:
+               case NES_AEQE_AEID_ROE_INVALID_RDMA_WRITE_OR_READ_RESP:
+                       printk(KERN_ERR PFX "QP[%u] async_event_id=0x%04X IB_EVENT_QP_FATAL\n",
+                                       nesqp->hwqp.qp_id, async_event_id);
+                       nes_terminate_connection(nesdev, nesqp, aeqe, IB_EVENT_QP_FATAL);
                        break;
+
                case NES_AEQE_AEID_CQ_OPERATION_ERROR:
                        context <<= 1;
                        nes_debug(NES_DBG_AEQ, "Processing an NES_AEQE_AEID_CQ_OPERATION_ERROR event on CQ%u, %p\n",
@@ -3133,83 +3586,19 @@ static void nes_process_iwarp_aeqe(struct nes_device *nesdev,
                        if (resource_allocated) {
                                printk(KERN_ERR PFX "%s: Processing an NES_AEQE_AEID_CQ_OPERATION_ERROR event on CQ%u\n",
                                                __func__, le32_to_cpu(aeqe->aeqe_words[NES_AEQE_COMP_QP_CQ_ID_IDX]));
+                               hw_cq = (struct nes_hw_cq *)(unsigned long)context;
+                               if (hw_cq) {
+                                       nescq = container_of(hw_cq, struct nes_cq, hw_cq);
+                                       if (nescq->ibcq.event_handler) {
+                                               ibevent.device = nescq->ibcq.device;
+                                               ibevent.event = IB_EVENT_CQ_ERR;
+                                               ibevent.element.cq = &nescq->ibcq;
+                                               nescq->ibcq.event_handler(&ibevent, nescq->ibcq.cq_context);
+                                       }
+                               }
                        }
                        break;
-               case NES_AEQE_AEID_DDP_UBE_DDP_MESSAGE_TOO_LONG_FOR_AVAILABLE_BUFFER:
-                       nesqp = nesadapter->qp_table[le32_to_cpu(
-                                       aeqe->aeqe_words[NES_AEQE_COMP_QP_CQ_ID_IDX])-NES_FIRST_QPN];
-                       spin_lock_irqsave(&nesqp->lock, flags);
-                       nesqp->hw_iwarp_state = iwarp_state;
-                       nesqp->hw_tcp_state = tcp_state;
-                       nesqp->last_aeq = async_event_id;
-                       spin_unlock_irqrestore(&nesqp->lock, flags);
-                       nes_debug(NES_DBG_AEQ, "Processing an NES_AEQE_AEID_DDP_UBE_DDP_MESSAGE_TOO_LONG"
-                                       "_FOR_AVAILABLE_BUFFER event on QP%u\n",
-                                       nesqp->hwqp.qp_id);
-                       if (nesqp->ibqp.event_handler) {
-                               ibevent.device = nesqp->ibqp.device;
-                               ibevent.element.qp = &nesqp->ibqp;
-                               ibevent.event = IB_EVENT_QP_ACCESS_ERR;
-                               nesqp->ibqp.event_handler(&ibevent, nesqp->ibqp.qp_context);
-                       }
-                       /* tell cm to disconnect, cm will queue work to thread */
-                       nes_cm_disconn(nesqp);
-                       break;
-               case NES_AEQE_AEID_DDP_UBE_INVALID_MSN_NO_BUFFER_AVAILABLE:
-                       nesqp = *((struct nes_qp **)&context);
-                       spin_lock_irqsave(&nesqp->lock, flags);
-                       nesqp->hw_iwarp_state = iwarp_state;
-                       nesqp->hw_tcp_state = tcp_state;
-                       nesqp->last_aeq = async_event_id;
-                       spin_unlock_irqrestore(&nesqp->lock, flags);
-                       nes_debug(NES_DBG_AEQ, "Processing an NES_AEQE_AEID_DDP_UBE_INVALID_MSN"
-                                       "_NO_BUFFER_AVAILABLE event on QP%u\n",
-                                       nesqp->hwqp.qp_id);
-                       if (nesqp->ibqp.event_handler) {
-                               ibevent.device = nesqp->ibqp.device;
-                               ibevent.element.qp = &nesqp->ibqp;
-                               ibevent.event = IB_EVENT_QP_FATAL;
-                               nesqp->ibqp.event_handler(&ibevent, nesqp->ibqp.qp_context);
-                       }
-                       /* tell cm to disconnect, cm will queue work to thread */
-                       nes_cm_disconn(nesqp);
-                       break;
-               case NES_AEQE_AEID_LLP_RECEIVED_MPA_CRC_ERROR:
-                       nesqp = *((struct nes_qp **)&context);
-                       spin_lock_irqsave(&nesqp->lock, flags);
-                       nesqp->hw_iwarp_state = iwarp_state;
-                       nesqp->hw_tcp_state = tcp_state;
-                       nesqp->last_aeq = async_event_id;
-                       spin_unlock_irqrestore(&nesqp->lock, flags);
-                       nes_debug(NES_DBG_AEQ, "Processing an NES_AEQE_AEID_LLP_RECEIVED_MPA_CRC_ERROR"
-                                       " event on QP%u \n  Q2 Data:\n",
-                                       nesqp->hwqp.qp_id);
-                       if (nesqp->ibqp.event_handler) {
-                               ibevent.device = nesqp->ibqp.device;
-                               ibevent.element.qp = &nesqp->ibqp;
-                               ibevent.event = IB_EVENT_QP_FATAL;
-                               nesqp->ibqp.event_handler(&ibevent, nesqp->ibqp.qp_context);
-                       }
-                       /* tell cm to disconnect, cm will queue work to thread */
-                       nes_cm_disconn(nesqp);
-                       break;
-                       /* TODO: additional AEs need to be here */
-               case NES_AEQE_AEID_AMP_BOUNDS_VIOLATION:
-                       nesqp = *((struct nes_qp **)&context);
-                       spin_lock_irqsave(&nesqp->lock, flags);
-                       nesqp->hw_iwarp_state = iwarp_state;
-                       nesqp->hw_tcp_state = tcp_state;
-                       nesqp->last_aeq = async_event_id;
-                       spin_unlock_irqrestore(&nesqp->lock, flags);
-                       if (nesqp->ibqp.event_handler) {
-                               ibevent.device = nesqp->ibqp.device;
-                               ibevent.element.qp = &nesqp->ibqp;
-                               ibevent.event = IB_EVENT_QP_ACCESS_ERR;
-                               nesqp->ibqp.event_handler(&ibevent,
-                                               nesqp->ibqp.qp_context);
-                       }
-                       nes_cm_disconn(nesqp);
-                       break;
+
                default:
                        nes_debug(NES_DBG_AEQ, "Processing an iWARP related AE for QP, misc = 0x%04X\n",
                                        async_event_id);
@@ -3218,7 +3607,6 @@ static void nes_process_iwarp_aeqe(struct nes_device *nesdev,
 
 }
 
-
 /**
  * nes_iwarp_ce_handler
  */
@@ -3353,6 +3741,8 @@ void flush_wqes(struct nes_device *nesdev, struct nes_qp *nesqp,
 {
        struct nes_cqp_request *cqp_request;
        struct nes_hw_cqp_wqe *cqp_wqe;
+       u32 sq_code = (NES_IWARP_CQE_MAJOR_FLUSH << 16) | NES_IWARP_CQE_MINOR_FLUSH;
+       u32 rq_code = (NES_IWARP_CQE_MAJOR_FLUSH << 16) | NES_IWARP_CQE_MINOR_FLUSH;
        int ret;
 
        cqp_request = nes_get_cqp_request(nesdev);
@@ -3369,6 +3759,24 @@ void flush_wqes(struct nes_device *nesdev, struct nes_qp *nesqp,
        cqp_wqe = &cqp_request->cqp_wqe;
        nes_fill_init_cqp_wqe(cqp_wqe, nesdev);
 
+       /* If wqe in error was identified, set code to be put into cqe */
+       if ((nesqp->term_sq_flush_code) && (which_wq & NES_CQP_FLUSH_SQ)) {
+               which_wq |= NES_CQP_FLUSH_MAJ_MIN;
+               sq_code = (CQE_MAJOR_DRV << 16) | nesqp->term_sq_flush_code;
+               nesqp->term_sq_flush_code = 0;
+       }
+
+       if ((nesqp->term_rq_flush_code) && (which_wq & NES_CQP_FLUSH_RQ)) {
+               which_wq |= NES_CQP_FLUSH_MAJ_MIN;
+               rq_code = (CQE_MAJOR_DRV << 16) | nesqp->term_rq_flush_code;
+               nesqp->term_rq_flush_code = 0;
+       }
+
+       if (which_wq & NES_CQP_FLUSH_MAJ_MIN) {
+               cqp_wqe->wqe_words[NES_CQP_QP_WQE_FLUSH_SQ_CODE] = cpu_to_le32(sq_code);
+               cqp_wqe->wqe_words[NES_CQP_QP_WQE_FLUSH_RQ_CODE] = cpu_to_le32(rq_code);
+       }
+
        cqp_wqe->wqe_words[NES_CQP_WQE_OPCODE_IDX] =
                        cpu_to_le32(NES_CQP_FLUSH_WQES | which_wq);
        cqp_wqe->wqe_words[NES_CQP_WQE_ID_IDX] = cpu_to_le32(nesqp->hwqp.qp_id);