1 files changed, 474 insertions, 574 deletions

diff --git a/drivers/net/netxen/netxen_nic_hw.c b/drivers/net/netxen/netxen_nic_hw.c
index 52a3798d8d94..b1cf46a0c48c 100644
--- a/drivers/net/netxen/netxen_nic_hw.c
+++ b/drivers/net/netxen/netxen_nic_hw.c
@@ -19,10 +19,11 @@
  * MA  02111-1307, USA.
  *
  * The full GNU General Public License is included in this distribution
- * in the file called LICENSE.
+ * in the file called "COPYING".
  *
  */
 
+#include <linux/slab.h>
 #include "netxen_nic.h"
 #include "netxen_nic_hw.h"
 
@@ -31,6 +32,7 @@
 #define MASK(n) ((1ULL<<(n))-1)
 #define MN_WIN(addr) (((addr & 0x1fc0000) >> 1) | ((addr >> 25) & 0x3ff))
 #define OCM_WIN(addr) (((addr & 0x1ff0000) >> 1) | ((addr >> 25) & 0x3ff))
+#define OCM_WIN_P3P(addr) (addr & 0xffc0000)
 #define MS_WIN(addr) (addr & 0x0ffc0000)
 
 #define GET_MEM_OFFS_2M(addr) (addr & MASK(18))
@@ -41,6 +43,11 @@
 #define CRB_HI(off)     ((crb_hub_agt[CRB_BLK(off)] << 20) | ((off) & 0xf0000))
 #define CRB_INDIRECT_2M (0x1e0000UL)
 
+static void netxen_nic_io_write_128M(struct netxen_adapter *adapter,
+                void __iomem *addr, u32 data);
+static u32 netxen_nic_io_read_128M(struct netxen_adapter *adapter,
+                void __iomem *addr);
+
 #ifndef readq
 static inline u64 readq(void __iomem *addr)
 {
@@ -326,7 +333,7 @@ netxen_pcie_sem_lock(struct netxen_adapter *adapter, int sem, u32 id_reg)
                 if (done == 1)
                         break;
                 if (++timeout >= NETXEN_PCIE_SEM_TIMEOUT)
-                        return -1;
+                        return -EIO;
                 msleep(1);
         }
 
@@ -339,8 +346,7 @@ netxen_pcie_sem_lock(struct netxen_adapter *adapter, int sem, u32 id_reg)
 void
 netxen_pcie_sem_unlock(struct netxen_adapter *adapter, int sem)
 {
-        int val;
-        val = NXRD32(adapter, NETXEN_PCIE_REG(PCIE_SEM_UNLOCK(sem)));
+        NXRD32(adapter, NETXEN_PCIE_REG(PCIE_SEM_UNLOCK(sem)));
 }
 
 int netxen_niu_xg_init_port(struct netxen_adapter *adapter, int port)
@@ -534,7 +540,7 @@ void netxen_p2_nic_set_multi(struct net_device *netdev)
         struct netxen_adapter *adapter = netdev_priv(netdev);
         struct dev_mc_list *mc_ptr;
         u8 null_addr[6];
-        int index = 0;
+        int i;
 
         memset(null_addr, 0, 6);
 
@@ -549,7 +555,7 @@ void netxen_p2_nic_set_multi(struct net_device *netdev)
                 return;
         }
 
-        if (netdev->mc_count == 0) {
+        if (netdev_mc_empty(netdev)) {
                 adapter->set_promisc(adapter,
                                 NETXEN_NIU_NON_PROMISC_MODE);
                 netxen_nic_disable_mcast_filter(adapter);
@@ -558,23 +564,20 @@ void netxen_p2_nic_set_multi(struct net_device *netdev)
 
         adapter->set_promisc(adapter, NETXEN_NIU_ALLMULTI_MODE);
         if (netdev->flags & IFF_ALLMULTI ||
-                        netdev->mc_count > adapter->max_mc_count) {
+                        netdev_mc_count(netdev) > adapter->max_mc_count) {
                 netxen_nic_disable_mcast_filter(adapter);
                 return;
         }
 
         netxen_nic_enable_mcast_filter(adapter);
 
-        for (mc_ptr = netdev->mc_list; mc_ptr; mc_ptr = mc_ptr->next, index++)
-                netxen_nic_set_mcast_addr(adapter, index, mc_ptr->dmi_addr);
-
-        if (index != netdev->mc_count)
-                printk(KERN_WARNING "%s: %s multicast address count mismatch\n",
-                        netxen_nic_driver_name, netdev->name);
+        i = 0;
+        netdev_for_each_mc_addr(mc_ptr, netdev)
+                netxen_nic_set_mcast_addr(adapter, i++, mc_ptr->dmi_addr);
 
         /* Clear out remaining addresses */
-        for (; index < adapter->max_mc_count; index++)
-                netxen_nic_set_mcast_addr(adapter, index, null_addr);
+        while (i < adapter->max_mc_count)
+                netxen_nic_set_mcast_addr(adapter, i++, null_addr);
 }
 
 static int
@@ -685,6 +688,9 @@ void netxen_p3_nic_set_multi(struct net_device *netdev)
         struct list_head *head;
         nx_mac_list_t *cur;
 
+        if (adapter->is_up != NETXEN_ADAPTER_UP_MAGIC)
+                return;
+
         list_splice_tail_init(&adapter->mac_list, &del_list);
 
         nx_p3_nic_add_mac(adapter, adapter->mac_addr, &del_list);
@@ -696,16 +702,14 @@ void netxen_p3_nic_set_multi(struct net_device *netdev)
         }
 
         if ((netdev->flags & IFF_ALLMULTI) ||
-                        (netdev->mc_count > adapter->max_mc_count)) {
+                        (netdev_mc_count(netdev) > adapter->max_mc_count)) {
                 mode = VPORT_MISS_MODE_ACCEPT_MULTI;
                 goto send_fw_cmd;
         }
 
-        if (netdev->mc_count > 0) {
-                for (mc_ptr = netdev->mc_list; mc_ptr;
-                     mc_ptr = mc_ptr->next) {
+        if (!netdev_mc_empty(netdev)) {
+                netdev_for_each_mc_addr(mc_ptr, netdev)
                         nx_p3_nic_add_mac(adapter, mc_ptr->dmi_addr, &del_list);
-                }
         }
 
 send_fw_cmd:
@@ -769,17 +773,20 @@ int netxen_p3_nic_set_mac_addr(struct netxen_adapter *adapter, u8 *addr)
 int netxen_config_intr_coalesce(struct netxen_adapter *adapter)
 {
         nx_nic_req_t req;
-        u64 word;
-        int rv;
+        u64 word[6];
+        int rv, i;
 
         memset(&req, 0, sizeof(nx_nic_req_t));
+        memset(word, 0, sizeof(word));
 
         req.qhdr = cpu_to_le64(NX_HOST_REQUEST << 23);
 
-        word = NETXEN_CONFIG_INTR_COALESCE | ((u64)adapter->portnum << 16);
-        req.req_hdr = cpu_to_le64(word);
+        word[0] = NETXEN_CONFIG_INTR_COALESCE | ((u64)adapter->portnum << 16);
+        req.req_hdr = cpu_to_le64(word[0]);
 
-        memcpy(&req.words[0], &adapter->coal, sizeof(adapter->coal));
+        memcpy(&word[0], &adapter->coal, sizeof(adapter->coal));
+        for (i = 0; i < 6; i++)
+                req.words[i] = cpu_to_le64(word[i]);
 
         rv = netxen_send_cmd_descs(adapter, (struct cmd_desc_type0 *)&req, 1);
         if (rv != 0) {
@@ -1025,7 +1032,7 @@ static int netxen_get_flash_block(struct netxen_adapter *adapter, int base,
         return 0;
 }
 
-int netxen_get_flash_mac_addr(struct netxen_adapter *adapter, __le64 *mac)
+int netxen_get_flash_mac_addr(struct netxen_adapter *adapter, u64 *mac)
 {
         __le32 *pmac = (__le32 *) mac;
         u32 offset;
@@ -1050,7 +1057,7 @@ int netxen_get_flash_mac_addr(struct netxen_adapter *adapter, __le64 *mac)
         return 0;
 }
 
-int netxen_p3_get_mac_addr(struct netxen_adapter *adapter, __le64 *mac)
+int netxen_p3_get_mac_addr(struct netxen_adapter *adapter, u64 *mac)
 {
         uint32_t crbaddr, mac_hi, mac_lo;
         int pci_func = adapter->ahw.pci_func;
@@ -1073,89 +1080,71 @@ int netxen_p3_get_mac_addr(struct netxen_adapter *adapter, __le64 *mac)
  * Changes the CRB window to the specified window.
  */
 static void
-netxen_nic_pci_change_crbwindow_128M(struct netxen_adapter *adapter, u32 wndw)
+netxen_nic_pci_set_crbwindow_128M(struct netxen_adapter *adapter,
+                u32 window)
 {
         void __iomem *offset;
-        u32 tmp;
-        int count = 0;
-        uint8_t func = adapter->ahw.pci_func;
+        int count = 10;
+        u8 func = adapter->ahw.pci_func;
 
-        if (adapter->curr_window == wndw)
+        if (adapter->ahw.crb_win == window)
                 return;
-        /*
-         * Move the CRB window.
-         * We need to write to the "direct access" region of PCI
-         * to avoid a race condition where the window register has
-         * not been successfully written across CRB before the target
-         * register address is received by PCI. The direct region bypasses
-         * the CRB bus.
-         */
+
         offset = PCI_OFFSET_SECOND_RANGE(adapter,
                         NETXEN_PCIX_PH_REG(PCIE_CRB_WINDOW_REG(func)));
 
-        if (wndw & 0x1)
-                wndw = NETXEN_WINDOW_ONE;
+        writel(window, offset);
+        do {
+                if (window == readl(offset))
+                        break;
 
-        writel(wndw, offset);
+                if (printk_ratelimit())
+                        dev_warn(&adapter->pdev->dev,
+                                        "failed to set CRB window to %d\n",
+                                        (window == NETXEN_WINDOW_ONE));
+                udelay(1);
 
-        /* MUST make sure window is set before we forge on... */
-        while ((tmp = readl(offset)) != wndw) {
-                printk(KERN_WARNING "%s: %s WARNING: CRB window value not "
-                       "registered properly: 0x%08x.\n",
-                       netxen_nic_driver_name, __func__, tmp);
-                mdelay(1);
-                if (count >= 10)
-                        break;
-                count++;
-        }
+        } while (--count > 0);
 
-        if (wndw == NETXEN_WINDOW_ONE)
-                adapter->curr_window = 1;
-        else
-                adapter->curr_window = 0;
+        if (count > 0)
+                adapter->ahw.crb_win = window;
 }
 
 /*
- * Return -1 if off is not valid,
+ * Returns < 0 if off is not valid,
  *       1 if window access is needed. 'off' is set to offset from
  *         CRB space in 128M pci map
  *       0 if no window access is needed. 'off' is set to 2M addr
  * In: 'off' is offset from base in 128M pci map
  */
 static int
-netxen_nic_pci_get_crb_addr_2M(struct netxen_adapter *adapter, ulong *off)
+netxen_nic_pci_get_crb_addr_2M(struct netxen_adapter *adapter,
+                ulong off, void __iomem **addr)
 {
         crb_128M_2M_sub_block_map_t *m;
 
 
-        if (*off >= NETXEN_CRB_MAX)
-                return -1;
-
-        if (*off >= NETXEN_PCI_CAMQM && (*off < NETXEN_PCI_CAMQM_2M_END)) {
-                *off = (*off - NETXEN_PCI_CAMQM) + NETXEN_PCI_CAMQM_2M_BASE +
-                        (ulong)adapter->ahw.pci_base0;
-                return 0;
-        }
-
-        if (*off < NETXEN_PCI_CRBSPACE)
-                return -1;
+        if ((off >= NETXEN_CRB_MAX) || (off < NETXEN_PCI_CRBSPACE))
+                return -EINVAL;
 
-        *off -= NETXEN_PCI_CRBSPACE;
+        off -= NETXEN_PCI_CRBSPACE;
 
         /*
          * Try direct map
          */
-        m = &crb_128M_2M_map[CRB_BLK(*off)].sub_block[CRB_SUBBLK(*off)];
+        m = &crb_128M_2M_map[CRB_BLK(off)].sub_block[CRB_SUBBLK(off)];
 
-        if (m->valid && (m->start_128M <= *off) && (m->end_128M > *off)) {
-                *off = *off + m->start_2M - m->start_128M +
-                        (ulong)adapter->ahw.pci_base0;
+        if (m->valid && (m->start_128M <= off) && (m->end_128M > off)) {
+                *addr = adapter->ahw.pci_base0 + m->start_2M +
+                        (off - m->start_128M);
                 return 0;
         }
 
         /*
          * Not in direct map, use crb window
          */
+        *addr = adapter->ahw.pci_base0 + CRB_INDIRECT_2M +
+                (off & MASK(16));
         return 1;
 }
 
@@ -1165,52 +1154,78 @@ netxen_nic_pci_get_crb_addr_2M(struct netxen_adapter *adapter, ulong *off)
  * side effect: lock crb window
  */
 static void
-netxen_nic_pci_set_crbwindow_2M(struct netxen_adapter *adapter, ulong *off)
+netxen_nic_pci_set_crbwindow_2M(struct netxen_adapter *adapter, ulong off)
 {
-        u32 win_read;
+        u32 window;
+        void __iomem *addr = adapter->ahw.pci_base0 + CRB_WINDOW_2M;
 
-        adapter->crb_win = CRB_HI(*off);
-        writel(adapter->crb_win, (adapter->ahw.pci_base0 + CRB_WINDOW_2M));
-        /*
-         * Read back value to make sure write has gone through before trying
-         * to use it.
-         */
-        win_read = readl(adapter->ahw.pci_base0 + CRB_WINDOW_2M);
-        if (win_read != adapter->crb_win) {
-                printk(KERN_ERR "%s: Written crbwin (0x%x) != "
-                                "Read crbwin (0x%x), off=0x%lx\n",
-                                __func__, adapter->crb_win, win_read, *off);
+        off -= NETXEN_PCI_CRBSPACE;
+
+        window = CRB_HI(off);
+
+        if (adapter->ahw.crb_win == window)
+                return;
+
+        writel(window, addr);
+        if (readl(addr) != window) {
+                if (printk_ratelimit())
+                        dev_warn(&adapter->pdev->dev,
+                                "failed to set CRB window to %d off 0x%lx\n",
+                                window, off);
         }
-        *off = (*off & MASK(16)) + CRB_INDIRECT_2M +
-                (ulong)adapter->ahw.pci_base0;
+        adapter->ahw.crb_win = window;
+}
+
+static void __iomem *
+netxen_nic_map_indirect_address_128M(struct netxen_adapter *adapter,
+                ulong win_off, void __iomem **mem_ptr)
+{
+        ulong off = win_off;
+        void __iomem *addr;
+        resource_size_t mem_base;
+
+        if (ADDR_IN_WINDOW1(win_off))
+                off = NETXEN_CRB_NORMAL(win_off);
+
+        addr = pci_base_offset(adapter, off);
+        if (addr)
+                return addr;
+
+        if (adapter->ahw.pci_len0 == 0)
+                off -= NETXEN_PCI_CRBSPACE;
+
+        mem_base = pci_resource_start(adapter->pdev, 0);
+        *mem_ptr = ioremap(mem_base + (off & PAGE_MASK), PAGE_SIZE);
+        if (*mem_ptr)
+                addr = *mem_ptr + (off & (PAGE_SIZE - 1));
+
+        return addr;
 }
 
 static int
 netxen_nic_hw_write_wx_128M(struct netxen_adapter *adapter, ulong off, u32 data)
 {
         unsigned long flags;
-        void __iomem *addr;
-
-        if (ADDR_IN_WINDOW1(off))
-                addr = NETXEN_CRB_NORMALIZE(adapter, off);
-        else
-                addr = pci_base_offset(adapter, off);
+        void __iomem *addr, *mem_ptr = NULL;
 
-        BUG_ON(!addr);
+        addr = netxen_nic_map_indirect_address_128M(adapter, off, &mem_ptr);
+        if (!addr)
+                return -EIO;
 
-        if (ADDR_IN_WINDOW1(off)) {     /* Window 1 */
-                read_lock(&adapter->adapter_lock);
-                writel(data, addr);
-                read_unlock(&adapter->adapter_lock);
-        } else {                /* Window 0 */
-                write_lock_irqsave(&adapter->adapter_lock, flags);
-                addr = pci_base_offset(adapter, off);
-                netxen_nic_pci_change_crbwindow_128M(adapter, 0);
+        if (ADDR_IN_WINDOW1(off)) { /* Window 1 */
+                netxen_nic_io_write_128M(adapter, addr, data);
+        } else {        /* Window 0 */
+                write_lock_irqsave(&adapter->ahw.crb_lock, flags);
+                netxen_nic_pci_set_crbwindow_128M(adapter, 0);
                 writel(data, addr);
-                netxen_nic_pci_change_crbwindow_128M(adapter, 1);
-                write_unlock_irqrestore(&adapter->adapter_lock, flags);
+                netxen_nic_pci_set_crbwindow_128M(adapter,
+                                NETXEN_WINDOW_ONE);
+                write_unlock_irqrestore(&adapter->ahw.crb_lock, flags);
         }
 
+        if (mem_ptr)
+                iounmap(mem_ptr);
+
         return 0;
 }
 
@@ -1218,28 +1233,27 @@ static u32
 netxen_nic_hw_read_wx_128M(struct netxen_adapter *adapter, ulong off)
 {
         unsigned long flags;
-        void __iomem *addr;
+        void __iomem *addr, *mem_ptr = NULL;
         u32 data;
 
-        if (ADDR_IN_WINDOW1(off))
-                addr = NETXEN_CRB_NORMALIZE(adapter, off);
-        else
-                addr = pci_base_offset(adapter, off);
-
-        BUG_ON(!addr);
+        addr = netxen_nic_map_indirect_address_128M(adapter, off, &mem_ptr);
+        if (!addr)
+                return -EIO;
 
-        if (ADDR_IN_WINDOW1(off)) {     /* Window 1 */
-                read_lock(&adapter->adapter_lock);
+        if (ADDR_IN_WINDOW1(off)) { /* Window 1 */
+                data = netxen_nic_io_read_128M(adapter, addr);
+        } else {        /* Window 0 */
+                write_lock_irqsave(&adapter->ahw.crb_lock, flags);
+                netxen_nic_pci_set_crbwindow_128M(adapter, 0);
                 data = readl(addr);
-                read_unlock(&adapter->adapter_lock);
-        } else {                /* Window 0 */
-                write_lock_irqsave(&adapter->adapter_lock, flags);
-                netxen_nic_pci_change_crbwindow_128M(adapter, 0);
-                data = readl(addr);
-                netxen_nic_pci_change_crbwindow_128M(adapter, 1);
-                write_unlock_irqrestore(&adapter->adapter_lock, flags);
+                netxen_nic_pci_set_crbwindow_128M(adapter,
+                                NETXEN_WINDOW_ONE);
+                write_unlock_irqrestore(&adapter->ahw.crb_lock, flags);
         }
 
+        if (mem_ptr)
+                iounmap(mem_ptr);
+
         return data;
 }
 
@@ -1248,28 +1262,30 @@ netxen_nic_hw_write_wx_2M(struct netxen_adapter *adapter, ulong off, u32 data)
 {
         unsigned long flags;
         int rv;
+        void __iomem *addr = NULL;
 
-        rv = netxen_nic_pci_get_crb_addr_2M(adapter, &off);
+        rv = netxen_nic_pci_get_crb_addr_2M(adapter, off, &addr);
 
-        if (rv == -1) {
-                printk(KERN_ERR "%s: invalid offset: 0x%016lx\n",
-                                __func__, off);
-                dump_stack();
-                return -1;
+        if (rv == 0) {
+                writel(data, addr);
+                return 0;
         }
 
-        if (rv == 1) {
-                write_lock_irqsave(&adapter->adapter_lock, flags);
+        if (rv > 0) {
+                /* indirect access */
+                write_lock_irqsave(&adapter->ahw.crb_lock, flags);
                 crb_win_lock(adapter);
-                netxen_nic_pci_set_crbwindow_2M(adapter, &off);
-                writel(data, (void __iomem *)off);
+                netxen_nic_pci_set_crbwindow_2M(adapter, off);
+                writel(data, addr);
                 crb_win_unlock(adapter);
-                write_unlock_irqrestore(&adapter->adapter_lock, flags);
-        } else
-                writel(data, (void __iomem *)off);
-
+                write_unlock_irqrestore(&adapter->ahw.crb_lock, flags);
+                return 0;
+        }
 
-        return 0;
+        dev_err(&adapter->pdev->dev,
+                        "%s: invalid offset: 0x%016lx\n", __func__, off);
+        dump_stack();
+        return -EIO;
 }
 
 static u32
@@ -1278,102 +1294,37 @@ netxen_nic_hw_read_wx_2M(struct netxen_adapter *adapter, ulong off)
         unsigned long flags;
         int rv;
         u32 data;
+        void __iomem *addr = NULL;
 
-        rv = netxen_nic_pci_get_crb_addr_2M(adapter, &off);
+        rv = netxen_nic_pci_get_crb_addr_2M(adapter, off, &addr);
 
-        if (rv == -1) {
-                printk(KERN_ERR "%s: invalid offset: 0x%016lx\n",
-                                __func__, off);
-                dump_stack();
-                return -1;
-        }
+        if (rv == 0)
+                return readl(addr);
 
-        if (rv == 1) {
-                write_lock_irqsave(&adapter->adapter_lock, flags);
+        if (rv > 0) {
+                /* indirect access */
+                write_lock_irqsave(&adapter->ahw.crb_lock, flags);
                 crb_win_lock(adapter);
-                netxen_nic_pci_set_crbwindow_2M(adapter, &off);
-                data = readl((void __iomem *)off);
+                netxen_nic_pci_set_crbwindow_2M(adapter, off);
+                data = readl(addr);
                 crb_win_unlock(adapter);
-                write_unlock_irqrestore(&adapter->adapter_lock, flags);
-        } else
-                data = readl((void __iomem *)off);
-
-        return data;
-}
-
-static int netxen_pci_set_window_warning_count;
-
-static unsigned long
-netxen_nic_pci_set_window_128M(struct netxen_adapter *adapter,
-                unsigned long long addr)
-{
-        void __iomem *offset;
-        int window;
-        unsigned long long      qdr_max;
-        uint8_t func = adapter->ahw.pci_func;
-
-        if (NX_IS_REVISION_P2(adapter->ahw.revision_id)) {
-                qdr_max = NETXEN_ADDR_QDR_NET_MAX_P2;
-        } else {
-                qdr_max = NETXEN_ADDR_QDR_NET_MAX_P3;
+                write_unlock_irqrestore(&adapter->ahw.crb_lock, flags);
+                return data;
         }
 
-        if (ADDR_IN_RANGE(addr, NETXEN_ADDR_DDR_NET, NETXEN_ADDR_DDR_NET_MAX)) {
-                /* DDR network side */
-                addr -= NETXEN_ADDR_DDR_NET;
-                window = (addr >> 25) & 0x3ff;
-                if (adapter->ahw.ddr_mn_window != window) {
-                        adapter->ahw.ddr_mn_window = window;
-                        offset = PCI_OFFSET_SECOND_RANGE(adapter,
-                                NETXEN_PCIX_PH_REG(PCIE_MN_WINDOW_REG(func)));
-                        writel(window, offset);
-                        /* MUST make sure window is set before we forge on... */
-                        readl(offset);
-                }
-                addr -= (window * NETXEN_WINDOW_ONE);
-                addr += NETXEN_PCI_DDR_NET;
-        } else if (ADDR_IN_RANGE(addr, NETXEN_ADDR_OCM0, NETXEN_ADDR_OCM0_MAX)) {
-                addr -= NETXEN_ADDR_OCM0;
-                addr += NETXEN_PCI_OCM0;
-        } else if (ADDR_IN_RANGE(addr, NETXEN_ADDR_OCM1, NETXEN_ADDR_OCM1_MAX)) {
-                addr -= NETXEN_ADDR_OCM1;
-                addr += NETXEN_PCI_OCM1;
-        } else if (ADDR_IN_RANGE(addr, NETXEN_ADDR_QDR_NET, qdr_max)) {
-                /* QDR network side */
-                addr -= NETXEN_ADDR_QDR_NET;
-                window = (addr >> 22) & 0x3f;
-                if (adapter->ahw.qdr_sn_window != window) {
-                        adapter->ahw.qdr_sn_window = window;
-                        offset = PCI_OFFSET_SECOND_RANGE(adapter,
-                                NETXEN_PCIX_PH_REG(PCIE_SN_WINDOW_REG(func)));
-                        writel((window << 22), offset);
-                        /* MUST make sure window is set before we forge on... */
-                        readl(offset);
-                }
-                addr -= (window * 0x400000);
-                addr += NETXEN_PCI_QDR_NET;
-        } else {
-                /*
-                 * peg gdb frequently accesses memory that doesn't exist,
-                 * this limits the chit chat so debugging isn't slowed down.
-                 */
-                if ((netxen_pci_set_window_warning_count++ < 8)
-                    || (netxen_pci_set_window_warning_count % 64 == 0))
-                        printk("%s: Warning:netxen_nic_pci_set_window()"
-                               " Unknown address range!\n",
-                               netxen_nic_driver_name);
-                addr = -1UL;
-        }
-        return addr;
+        dev_err(&adapter->pdev->dev,
+                        "%s: invalid offset: 0x%016lx\n", __func__, off);
+        dump_stack();
+        return -1;
 }
 
 /* window 1 registers only */
 static void netxen_nic_io_write_128M(struct netxen_adapter *adapter,
                 void __iomem *addr, u32 data)
 {
-        read_lock(&adapter->adapter_lock);
+        read_lock(&adapter->ahw.crb_lock);
         writel(data, addr);
-        read_unlock(&adapter->adapter_lock);
+        read_unlock(&adapter->ahw.crb_lock);
 }
 
 static u32 netxen_nic_io_read_128M(struct netxen_adapter *adapter,
@@ -1381,9 +1332,9 @@ static u32 netxen_nic_io_read_128M(struct netxen_adapter *adapter,
 {
         u32 val;
 
-        read_lock(&adapter->adapter_lock);
+        read_lock(&adapter->ahw.crb_lock);
         val = readl(addr);
-        read_unlock(&adapter->adapter_lock);
+        read_unlock(&adapter->ahw.crb_lock);
 
         return val;
 }
@@ -1403,488 +1354,437 @@ static u32 netxen_nic_io_read_2M(struct netxen_adapter *adapter,
 void __iomem *
 netxen_get_ioaddr(struct netxen_adapter *adapter, u32 offset)
 {
-        ulong off = offset;
+        void __iomem *addr = NULL;
 
         if (NX_IS_REVISION_P2(adapter->ahw.revision_id)) {
-                if (offset < NETXEN_CRB_PCIX_HOST2 &&
-                                offset > NETXEN_CRB_PCIX_HOST)
-                        return PCI_OFFSET_SECOND_RANGE(adapter, offset);
-                return NETXEN_CRB_NORMALIZE(adapter, offset);
+                if ((offset < NETXEN_CRB_PCIX_HOST2) &&
+                                (offset > NETXEN_CRB_PCIX_HOST))
+                        addr = PCI_OFFSET_SECOND_RANGE(adapter, offset);
+                else
+                        addr = NETXEN_CRB_NORMALIZE(adapter, offset);
+        } else {
+                WARN_ON(netxen_nic_pci_get_crb_addr_2M(adapter,
+                                        offset, &addr));
         }
 
-        BUG_ON(netxen_nic_pci_get_crb_addr_2M(adapter, &off));
-        return (void __iomem *)off;
+        return addr;
 }
 
-static unsigned long
-netxen_nic_pci_set_window_2M(struct netxen_adapter *adapter,
-                unsigned long long addr)
+static int
+netxen_nic_pci_set_window_128M(struct netxen_adapter *adapter,
+                u64 addr, u32 *start)
 {
-        int window;
-        u32 win_read;
-
-        if (ADDR_IN_RANGE(addr, NETXEN_ADDR_DDR_NET, NETXEN_ADDR_DDR_NET_MAX)) {
-                /* DDR network side */
-                window = MN_WIN(addr);
-                adapter->ahw.ddr_mn_window = window;
-                NXWR32(adapter, adapter->ahw.mn_win_crb, window);
-                win_read = NXRD32(adapter, adapter->ahw.mn_win_crb);
-                if ((win_read << 17) != window) {
-                        printk(KERN_INFO "Written MNwin (0x%x) != "
-                                "Read MNwin (0x%x)\n", window, win_read);
-                }
-                addr = GET_MEM_OFFS_2M(addr) + NETXEN_PCI_DDR_NET;
+        if (ADDR_IN_RANGE(addr, NETXEN_ADDR_OCM0, NETXEN_ADDR_OCM0_MAX)) {
+                *start = (addr - NETXEN_ADDR_OCM0  + NETXEN_PCI_OCM0);
+                return 0;
         } else if (ADDR_IN_RANGE(addr,
-                                NETXEN_ADDR_OCM0, NETXEN_ADDR_OCM0_MAX)) {
-                if ((addr & 0x00ff800) == 0xff800) {
-                        printk("%s: QM access not handled.\n", __func__);
-                        addr = -1UL;
-                }
+                                NETXEN_ADDR_OCM1, NETXEN_ADDR_OCM1_MAX)) {
+                *start = (addr - NETXEN_ADDR_OCM1 + NETXEN_PCI_OCM1);
+                return 0;
+        }
+
+        return -EIO;
+}
 
+static int
+netxen_nic_pci_set_window_2M(struct netxen_adapter *adapter,
+                u64 addr, u32 *start)
+{
+        u32 window;
+        struct pci_dev *pdev = adapter->pdev;
+
+        if ((addr & 0x00ff800) == 0xff800) {
+                if (printk_ratelimit())
+                        dev_warn(&pdev->dev, "QM access not handled\n");
+                return -EIO;
+        }
+
+        if (NX_IS_REVISION_P3P(adapter->ahw.revision_id))
+                window = OCM_WIN_P3P(addr);
+        else
                 window = OCM_WIN(addr);
-                adapter->ahw.ddr_mn_window = window;
-                NXWR32(adapter, adapter->ahw.mn_win_crb, window);
-                win_read = NXRD32(adapter, adapter->ahw.mn_win_crb);
-                if ((win_read >> 7) != window) {
-                        printk(KERN_INFO "%s: Written OCMwin (0x%x) != "
-                                        "Read OCMwin (0x%x)\n",
-                                        __func__, window, win_read);
-                }
-                addr = GET_MEM_OFFS_2M(addr) + NETXEN_PCI_OCM0_2M;
 
-        } else if (ADDR_IN_RANGE(addr,
-                        NETXEN_ADDR_QDR_NET, NETXEN_ADDR_QDR_NET_MAX_P3)) {
-                /* QDR network side */
-                window = MS_WIN(addr);
-                adapter->ahw.qdr_sn_window = window;
-                NXWR32(adapter, adapter->ahw.ms_win_crb, window);
-                win_read = NXRD32(adapter, adapter->ahw.ms_win_crb);
-                if (win_read != window) {
-                        printk(KERN_INFO "%s: Written MSwin (0x%x) != "
-                                        "Read MSwin (0x%x)\n",
-                                        __func__, window, win_read);
-                }
-                addr = GET_MEM_OFFS_2M(addr) + NETXEN_PCI_QDR_NET;
+        writel(window, adapter->ahw.ocm_win_crb);
+        /* read back to flush */
+        readl(adapter->ahw.ocm_win_crb);
 
-        } else {
-                /*
-                 * peg gdb frequently accesses memory that doesn't exist,
-                 * this limits the chit chat so debugging isn't slowed down.
-                 */
-                if ((netxen_pci_set_window_warning_count++ < 8)
-                        || (netxen_pci_set_window_warning_count%64 == 0)) {
-                        printk("%s: Warning:%s Unknown address range!\n",
-                                        __func__, netxen_nic_driver_name);
+        adapter->ahw.ocm_win = window;
+        *start = NETXEN_PCI_OCM0_2M + GET_MEM_OFFS_2M(addr);
+        return 0;
 }
-                addr = -1UL;
+
+static int
+netxen_nic_pci_mem_access_direct(struct netxen_adapter *adapter, u64 off,
+                u64 *data, int op)
+{
+        void __iomem *addr, *mem_ptr = NULL;
+        resource_size_t mem_base;
+        int ret = -EIO;
+        u32 start;
+
+        spin_lock(&adapter->ahw.mem_lock);
+
+        ret = adapter->pci_set_window(adapter, off, &start);
+        if (ret != 0)
+                goto unlock;
+
+        addr = pci_base_offset(adapter, start);
+        if (addr)
+                goto noremap;
+
+        mem_base = pci_resource_start(adapter->pdev, 0) + (start & PAGE_MASK);
+
+        mem_ptr = ioremap(mem_base, PAGE_SIZE);
+        if (mem_ptr == NULL) {
+                ret = -EIO;
+                goto unlock;
         }
-        return addr;
+
+        addr = mem_ptr + (start & (PAGE_SIZE - 1));
+
+noremap:
+        if (op == 0)    /* read */
+                *data = readq(addr);
+        else            /* write */
+                writeq(*data, addr);
+
+unlock:
+        spin_unlock(&adapter->ahw.mem_lock);
+
+        if (mem_ptr)
+                iounmap(mem_ptr);
+        return ret;
 }
 
 #define MAX_CTL_CHECK   1000
 
 static int
 netxen_nic_pci_mem_write_128M(struct netxen_adapter *adapter,
-                u64 off, void *data, int size)
+                u64 off, u64 data)
 {
-        unsigned long   flags;
-        int          i, j, ret = 0, loop, sz[2], off0;
-        uint32_t      temp;
-        uint64_t      off8, tmpw, word[2] = {0, 0};
+        int j, ret;
+        u32 temp, off_lo, off_hi, addr_hi, data_hi, data_lo;
         void __iomem *mem_crb;
 
-        if (size != 8)
+        /* Only 64-bit aligned access */
+        if (off & 7)
                 return -EIO;
 
+        /* P2 has different SIU and MIU test agent base addr */
         if (ADDR_IN_RANGE(off, NETXEN_ADDR_QDR_NET,
                                 NETXEN_ADDR_QDR_NET_MAX_P2)) {
-                mem_crb = pci_base_offset(adapter, NETXEN_CRB_QDR_NET);
+                mem_crb = pci_base_offset(adapter,
+                                NETXEN_CRB_QDR_NET+SIU_TEST_AGT_BASE);
+                addr_hi = SIU_TEST_AGT_ADDR_HI;
+                data_lo = SIU_TEST_AGT_WRDATA_LO;
+                data_hi = SIU_TEST_AGT_WRDATA_HI;
+                off_lo = off & SIU_TEST_AGT_ADDR_MASK;
+                off_hi = SIU_TEST_AGT_UPPER_ADDR(off);
                 goto correct;
         }
 
         if (ADDR_IN_RANGE(off, NETXEN_ADDR_DDR_NET, NETXEN_ADDR_DDR_NET_MAX)) {
-                mem_crb = pci_base_offset(adapter, NETXEN_CRB_DDR_NET);
+                mem_crb = pci_base_offset(adapter,
+                                NETXEN_CRB_DDR_NET+MIU_TEST_AGT_BASE);
+                addr_hi = MIU_TEST_AGT_ADDR_HI;
+                data_lo = MIU_TEST_AGT_WRDATA_LO;
+                data_hi = MIU_TEST_AGT_WRDATA_HI;
+                off_lo = off & MIU_TEST_AGT_ADDR_MASK;
+                off_hi = 0;
                 goto correct;
         }
 
-        return -EIO;
-
-correct:
-        off8 = off & 0xfffffff8;
-        off0 = off & 0x7;
-        sz[0] = (size < (8 - off0)) ? size : (8 - off0);
-        sz[1] = size - sz[0];
-        loop = ((off0 + size - 1) >> 3) + 1;
-
-        if ((size != 8) || (off0 != 0))  {
-                for (i = 0; i < loop; i++) {
-                        if (adapter->pci_mem_read(adapter,
-                                off8 + (i << 3), &word[i], 8))
-                                return -1;
+        if (ADDR_IN_RANGE(off, NETXEN_ADDR_OCM0, NETXEN_ADDR_OCM0_MAX) ||
+                ADDR_IN_RANGE(off, NETXEN_ADDR_OCM1, NETXEN_ADDR_OCM1_MAX)) {
+                if (adapter->ahw.pci_len0 != 0) {
+                        return netxen_nic_pci_mem_access_direct(adapter,
+                                        off, &data, 1);
                 }
         }
 
-        switch (size) {
-        case 1:
-                tmpw = *((uint8_t *)data);
-                break;
-        case 2:
-                tmpw = *((uint16_t *)data);
-                break;
-        case 4:
-                tmpw = *((uint32_t *)data);
-                break;
-        case 8:
-        default:
-                tmpw = *((uint64_t *)data);
-                break;
-        }
-        word[0] &= ~((~(~0ULL << (sz[0] * 8))) << (off0 * 8));
-        word[0] |= tmpw << (off0 * 8);
+        return -EIO;
 
-        if (loop == 2) {
-                word[1] &= ~(~0ULL << (sz[1] * 8));
-                word[1] |= tmpw >> (sz[0] * 8);
+correct:
+        spin_lock(&adapter->ahw.mem_lock);
+        netxen_nic_pci_set_crbwindow_128M(adapter, 0);
+
+        writel(off_lo, (mem_crb + MIU_TEST_AGT_ADDR_LO));
+        writel(off_hi, (mem_crb + addr_hi));
+        writel(data & 0xffffffff, (mem_crb + data_lo));
+        writel((data >> 32) & 0xffffffff, (mem_crb + data_hi));
+        writel((TA_CTL_ENABLE | TA_CTL_WRITE), (mem_crb + TEST_AGT_CTRL));
+        writel((TA_CTL_START | TA_CTL_ENABLE | TA_CTL_WRITE),
+                        (mem_crb + TEST_AGT_CTRL));
+
+        for (j = 0; j < MAX_CTL_CHECK; j++) {
+                temp = readl((mem_crb + TEST_AGT_CTRL));
+                if ((temp & TA_CTL_BUSY) == 0)
+                        break;
         }
 
-        write_lock_irqsave(&adapter->adapter_lock, flags);
-        netxen_nic_pci_change_crbwindow_128M(adapter, 0);
-
-        for (i = 0; i < loop; i++) {
-                writel((uint32_t)(off8 + (i << 3)),
-                        (mem_crb+MIU_TEST_AGT_ADDR_LO));
-                writel(0,
-                        (mem_crb+MIU_TEST_AGT_ADDR_HI));
-                writel(word[i] & 0xffffffff,
-                        (mem_crb+MIU_TEST_AGT_WRDATA_LO));
-                writel((word[i] >> 32) & 0xffffffff,
-                        (mem_crb+MIU_TEST_AGT_WRDATA_HI));
-                writel(MIU_TA_CTL_ENABLE|MIU_TA_CTL_WRITE,
-                        (mem_crb+MIU_TEST_AGT_CTRL));
-                writel(MIU_TA_CTL_START|MIU_TA_CTL_ENABLE|MIU_TA_CTL_WRITE,
-                        (mem_crb+MIU_TEST_AGT_CTRL));
-
-                for (j = 0; j < MAX_CTL_CHECK; j++) {
-                        temp = readl(
-                             (mem_crb+MIU_TEST_AGT_CTRL));
-                        if ((temp & MIU_TA_CTL_BUSY) == 0)
-                                break;
-                }
-
-                if (j >= MAX_CTL_CHECK) {
-                        if (printk_ratelimit())
-                                dev_err(&adapter->pdev->dev,
+        if (j >= MAX_CTL_CHECK) {
+                if (printk_ratelimit())
+                        dev_err(&adapter->pdev->dev,
                                         "failed to write through agent\n");
-                        ret = -1;
-                        break;
-                }
-        }
+                ret = -EIO;
+        } else
+                ret = 0;
 
-        netxen_nic_pci_change_crbwindow_128M(adapter, 1);
-        write_unlock_irqrestore(&adapter->adapter_lock, flags);
+        netxen_nic_pci_set_crbwindow_128M(adapter, NETXEN_WINDOW_ONE);
+        spin_unlock(&adapter->ahw.mem_lock);
         return ret;
 }
 
 static int
 netxen_nic_pci_mem_read_128M(struct netxen_adapter *adapter,
-                u64 off, void *data, int size)
+                u64 off, u64 *data)
 {
-        unsigned long   flags;
-        int          i, j = 0, k, start, end, loop, sz[2], off0[2];
-        uint32_t      temp;
-        uint64_t      off8, val, word[2] = {0, 0};
+        int j, ret;
+        u32 temp, off_lo, off_hi, addr_hi, data_hi, data_lo;
+        u64 val;
         void __iomem *mem_crb;
 
-        if (size != 8)
+        /* Only 64-bit aligned access */
+        if (off & 7)
                 return -EIO;
 
+        /* P2 has different SIU and MIU test agent base addr */
         if (ADDR_IN_RANGE(off, NETXEN_ADDR_QDR_NET,
                                 NETXEN_ADDR_QDR_NET_MAX_P2)) {
-                mem_crb = pci_base_offset(adapter, NETXEN_CRB_QDR_NET);
+                mem_crb = pci_base_offset(adapter,
+                                NETXEN_CRB_QDR_NET+SIU_TEST_AGT_BASE);
+                addr_hi = SIU_TEST_AGT_ADDR_HI;
+                data_lo = SIU_TEST_AGT_RDDATA_LO;
+                data_hi = SIU_TEST_AGT_RDDATA_HI;
+                off_lo = off & SIU_TEST_AGT_ADDR_MASK;
+                off_hi = SIU_TEST_AGT_UPPER_ADDR(off);
                 goto correct;
         }
 
         if (ADDR_IN_RANGE(off, NETXEN_ADDR_DDR_NET, NETXEN_ADDR_DDR_NET_MAX)) {
-                mem_crb = pci_base_offset(adapter, NETXEN_CRB_DDR_NET);
+                mem_crb = pci_base_offset(adapter,
+                                NETXEN_CRB_DDR_NET+MIU_TEST_AGT_BASE);
+                addr_hi = MIU_TEST_AGT_ADDR_HI;
+                data_lo = MIU_TEST_AGT_RDDATA_LO;
+                data_hi = MIU_TEST_AGT_RDDATA_HI;
+                off_lo = off & MIU_TEST_AGT_ADDR_MASK;
+                off_hi = 0;
                 goto correct;
         }
 
+        if (ADDR_IN_RANGE(off, NETXEN_ADDR_OCM0, NETXEN_ADDR_OCM0_MAX) ||
+                ADDR_IN_RANGE(off, NETXEN_ADDR_OCM1, NETXEN_ADDR_OCM1_MAX)) {
+                if (adapter->ahw.pci_len0 != 0) {
+                        return netxen_nic_pci_mem_access_direct(adapter,
+                                        off, data, 0);
+                }
+        }
+
         return -EIO;
 
 correct:
-        off8 = off & 0xfffffff8;
-        off0[0] = off & 0x7;
-        off0[1] = 0;
-        sz[0] = (size < (8 - off0[0])) ? size : (8 - off0[0]);
-        sz[1] = size - sz[0];
-        loop = ((off0[0] + size - 1) >> 3) + 1;
-
-        write_lock_irqsave(&adapter->adapter_lock, flags);
-        netxen_nic_pci_change_crbwindow_128M(adapter, 0);
-
-        for (i = 0; i < loop; i++) {
-                writel((uint32_t)(off8 + (i << 3)),
-                        (mem_crb+MIU_TEST_AGT_ADDR_LO));
-                writel(0,
-                        (mem_crb+MIU_TEST_AGT_ADDR_HI));
-                writel(MIU_TA_CTL_ENABLE,
-                        (mem_crb+MIU_TEST_AGT_CTRL));
-                writel(MIU_TA_CTL_START|MIU_TA_CTL_ENABLE,
-                        (mem_crb+MIU_TEST_AGT_CTRL));
+        spin_lock(&adapter->ahw.mem_lock);
+        netxen_nic_pci_set_crbwindow_128M(adapter, 0);
 
-                for (j = 0; j < MAX_CTL_CHECK; j++) {
-                        temp = readl(
-                              (mem_crb+MIU_TEST_AGT_CTRL));
-                        if ((temp & MIU_TA_CTL_BUSY) == 0)
-                                break;
-                }
+        writel(off_lo, (mem_crb + MIU_TEST_AGT_ADDR_LO));
+        writel(off_hi, (mem_crb + addr_hi));
+        writel(TA_CTL_ENABLE, (mem_crb + TEST_AGT_CTRL));
+        writel((TA_CTL_START|TA_CTL_ENABLE), (mem_crb + TEST_AGT_CTRL));
 
-                if (j >= MAX_CTL_CHECK) {
-                        if (printk_ratelimit())
-                                dev_err(&adapter->pdev->dev,
-                                        "failed to read through agent\n");
+        for (j = 0; j < MAX_CTL_CHECK; j++) {
+                temp = readl(mem_crb + TEST_AGT_CTRL);
+                if ((temp & TA_CTL_BUSY) == 0)
                         break;
-                }
-
-                start = off0[i] >> 2;
-                end   = (off0[i] + sz[i] - 1) >> 2;
-                for (k = start; k <= end; k++) {
-                        word[i] |= ((uint64_t) readl(
-                                    (mem_crb +
-                                    MIU_TEST_AGT_RDDATA(k))) << (32*k));
-                }
         }
 
-        netxen_nic_pci_change_crbwindow_128M(adapter, 1);
-        write_unlock_irqrestore(&adapter->adapter_lock, flags);
-
-        if (j >= MAX_CTL_CHECK)
-                return -1;
-
-        if (sz[0] == 8) {
-                val = word[0];
+        if (j >= MAX_CTL_CHECK) {
+                if (printk_ratelimit())
+                        dev_err(&adapter->pdev->dev,
+                                        "failed to read through agent\n");
+                ret = -EIO;
         } else {
-                val = ((word[0] >> (off0[0] * 8)) & (~(~0ULL << (sz[0] * 8)))) |
-                        ((word[1] & (~(~0ULL << (sz[1] * 8)))) << (sz[0] * 8));
-        }
 
-        switch (size) {
-        case 1:
-                *(uint8_t  *)data = val;
-                break;
-        case 2:
-                *(uint16_t *)data = val;
-                break;
-        case 4:
-                *(uint32_t *)data = val;
-                break;
-        case 8:
-                *(uint64_t *)data = val;
-                break;
+                temp = readl(mem_crb + data_hi);
+                val = ((u64)temp << 32);
+                val |= readl(mem_crb + data_lo);
+                *data = val;
+                ret = 0;
         }
-        return 0;
+
+        netxen_nic_pci_set_crbwindow_128M(adapter, NETXEN_WINDOW_ONE);
+        spin_unlock(&adapter->ahw.mem_lock);
+
+        return ret;
 }
 
 static int
 netxen_nic_pci_mem_write_2M(struct netxen_adapter *adapter,
-                u64 off, void *data, int size)
+                u64 off, u64 data)
 {
-        int i, j, ret = 0, loop, sz[2], off0;
-        uint32_t temp;
-        uint64_t off8, tmpw, word[2] = {0, 0};
+        int i, j, ret;
+        u32 temp, off8;
+        u64 stride;
         void __iomem *mem_crb;
 
-        if (size != 8)
+        /* Only 64-bit aligned access */
+        if (off & 7)
                 return -EIO;
 
+        /* P3 onward, test agent base for MIU and SIU is same */
         if (ADDR_IN_RANGE(off, NETXEN_ADDR_QDR_NET,
                                 NETXEN_ADDR_QDR_NET_MAX_P3)) {
-                mem_crb = netxen_get_ioaddr(adapter, NETXEN_CRB_QDR_NET);
+                mem_crb = netxen_get_ioaddr(adapter,
+                                NETXEN_CRB_QDR_NET+MIU_TEST_AGT_BASE);
                 goto correct;
         }
 
         if (ADDR_IN_RANGE(off, NETXEN_ADDR_DDR_NET, NETXEN_ADDR_DDR_NET_MAX)) {
-                mem_crb = netxen_get_ioaddr(adapter, NETXEN_CRB_DDR_NET);
+                mem_crb = netxen_get_ioaddr(adapter,
+                                NETXEN_CRB_DDR_NET+MIU_TEST_AGT_BASE);
                 goto correct;
         }
 
+        if (ADDR_IN_RANGE(off, NETXEN_ADDR_OCM0, NETXEN_ADDR_OCM0_MAX))
+                return netxen_nic_pci_mem_access_direct(adapter, off, &data, 1);
+
         return -EIO;
 
 correct:
-        off8 = off & 0xfffffff8;
-        off0 = off & 0x7;
-        sz[0] = (size < (8 - off0)) ? size : (8 - off0);
-        sz[1] = size - sz[0];
-        loop = ((off0 + size - 1) >> 3) + 1;
-
-        if ((size != 8) || (off0 != 0)) {
-                for (i = 0; i < loop; i++) {
-                        if (adapter->pci_mem_read(adapter,
-                                        off8 + (i << 3), &word[i], 8))
-                                return -1;
-                }
-        }
+        stride = NX_IS_REVISION_P3P(adapter->ahw.revision_id) ? 16 : 8;
 
-        switch (size) {
-        case 1:
-                tmpw = *((uint8_t *)data);
-                break;
-        case 2:
-                tmpw = *((uint16_t *)data);
-                break;
-        case 4:
-                tmpw = *((uint32_t *)data);
-                break;
-        case 8:
-        default:
-                tmpw = *((uint64_t *)data);
-        break;
-        }
+        off8 = off & ~(stride-1);
 
-        word[0] &= ~((~(~0ULL << (sz[0] * 8))) << (off0 * 8));
-        word[0] |= tmpw << (off0 * 8);
+        spin_lock(&adapter->ahw.mem_lock);
 
-        if (loop == 2) {
-                word[1] &= ~(~0ULL << (sz[1] * 8));
-                word[1] |= tmpw >> (sz[0] * 8);
-        }
-
-        /*
-         * don't lock here - write_wx gets the lock if each time
-         * write_lock_irqsave(&adapter->adapter_lock, flags);
-         * netxen_nic_pci_change_crbwindow_128M(adapter, 0);
-         */
+        writel(off8, (mem_crb + MIU_TEST_AGT_ADDR_LO));
+        writel(0, (mem_crb + MIU_TEST_AGT_ADDR_HI));
 
-        for (i = 0; i < loop; i++) {
-                writel(off8 + (i << 3), mem_crb+MIU_TEST_AGT_ADDR_LO);
-                writel(0, mem_crb+MIU_TEST_AGT_ADDR_HI);
-                writel(word[i] & 0xffffffff, mem_crb+MIU_TEST_AGT_WRDATA_LO);
-                writel((word[i] >> 32) & 0xffffffff,
-                                mem_crb+MIU_TEST_AGT_WRDATA_HI);
-                writel((MIU_TA_CTL_ENABLE | MIU_TA_CTL_WRITE),
-                                mem_crb+MIU_TEST_AGT_CTRL);
-                writel(MIU_TA_CTL_START | MIU_TA_CTL_ENABLE | MIU_TA_CTL_WRITE,
-                                mem_crb+MIU_TEST_AGT_CTRL);
+        i = 0;
+        if (stride == 16) {
+                writel(TA_CTL_ENABLE, (mem_crb + TEST_AGT_CTRL));
+                writel((TA_CTL_START | TA_CTL_ENABLE),
+                                (mem_crb + TEST_AGT_CTRL));
 
                 for (j = 0; j < MAX_CTL_CHECK; j++) {
-                        temp = readl(mem_crb + MIU_TEST_AGT_CTRL);
-                        if ((temp & MIU_TA_CTL_BUSY) == 0)
+                        temp = readl(mem_crb + TEST_AGT_CTRL);
+                        if ((temp & TA_CTL_BUSY) == 0)
                                 break;
                 }
 
                 if (j >= MAX_CTL_CHECK) {
-                        if (printk_ratelimit())
-                                dev_err(&adapter->pdev->dev,
-                                        "failed to write through agent\n");
-                        ret = -1;
-                        break;
+                        ret = -EIO;
+                        goto done;
                 }
+
+                i = (off & 0xf) ? 0 : 2;
+                writel(readl(mem_crb + MIU_TEST_AGT_RDDATA(i)),
+                                mem_crb + MIU_TEST_AGT_WRDATA(i));
+                writel(readl(mem_crb + MIU_TEST_AGT_RDDATA(i+1)),
+                                mem_crb + MIU_TEST_AGT_WRDATA(i+1));
+                i = (off & 0xf) ? 2 : 0;
         }
 
-        /*
-         * netxen_nic_pci_change_crbwindow_128M(adapter, 1);
-         * write_unlock_irqrestore(&adapter->adapter_lock, flags);
-         */
+        writel(data & 0xffffffff,
+                        mem_crb + MIU_TEST_AGT_WRDATA(i));
+        writel((data >> 32) & 0xffffffff,
+                        mem_crb + MIU_TEST_AGT_WRDATA(i+1));
+
+        writel((TA_CTL_ENABLE | TA_CTL_WRITE), (mem_crb + TEST_AGT_CTRL));
+        writel((TA_CTL_START | TA_CTL_ENABLE | TA_CTL_WRITE),
+                        (mem_crb + TEST_AGT_CTRL));
+
+        for (j = 0; j < MAX_CTL_CHECK; j++) {
+                temp = readl(mem_crb + TEST_AGT_CTRL);
+                if ((temp & TA_CTL_BUSY) == 0)
+                        break;
+        }
+
+        if (j >= MAX_CTL_CHECK) {
+                if (printk_ratelimit())
+                        dev_err(&adapter->pdev->dev,
+                                        "failed to write through agent\n");
+                ret = -EIO;
+        } else
+                ret = 0;
+
+done:
+        spin_unlock(&adapter->ahw.mem_lock);
+
         return ret;
 }
 
 static int
 netxen_nic_pci_mem_read_2M(struct netxen_adapter *adapter,
-                u64 off, void *data, int size)
+                u64 off, u64 *data)
 {
-        int i, j = 0, k, start, end, loop, sz[2], off0[2];
-        uint32_t      temp;
-        uint64_t      off8, val, word[2] = {0, 0};
+        int j, ret;
+        u32 temp, off8;
+        u64 val, stride;
         void __iomem *mem_crb;
 
-        if (size != 8)
+        /* Only 64-bit aligned access */
+        if (off & 7)
                 return -EIO;
 
+        /* P3 onward, test agent base for MIU and SIU is same */
         if (ADDR_IN_RANGE(off, NETXEN_ADDR_QDR_NET,
                                 NETXEN_ADDR_QDR_NET_MAX_P3)) {
-                mem_crb = netxen_get_ioaddr(adapter, NETXEN_CRB_QDR_NET);
+                mem_crb = netxen_get_ioaddr(adapter,
+                                NETXEN_CRB_QDR_NET+MIU_TEST_AGT_BASE);
                 goto correct;
         }
 
         if (ADDR_IN_RANGE(off, NETXEN_ADDR_DDR_NET, NETXEN_ADDR_DDR_NET_MAX)) {
-                mem_crb = netxen_get_ioaddr(adapter, NETXEN_CRB_DDR_NET);
+                mem_crb = netxen_get_ioaddr(adapter,
+                                NETXEN_CRB_DDR_NET+MIU_TEST_AGT_BASE);
                 goto correct;
         }
 
+        if (ADDR_IN_RANGE(off, NETXEN_ADDR_OCM0, NETXEN_ADDR_OCM0_MAX)) {
+                return netxen_nic_pci_mem_access_direct(adapter,
+                                off, data, 0);
+        }
+
         return -EIO;
 
 correct:
-        off8 = off & 0xfffffff8;
-        off0[0] = off & 0x7;
-        off0[1] = 0;
-        sz[0] = (size < (8 - off0[0])) ? size : (8 - off0[0]);
-        sz[1] = size - sz[0];
-        loop = ((off0[0] + size - 1) >> 3) + 1;
+        stride = NX_IS_REVISION_P3P(adapter->ahw.revision_id) ? 16 : 8;
 
-        /*
-         * don't lock here - write_wx gets the lock if each time
-         * write_lock_irqsave(&adapter->adapter_lock, flags);
-         * netxen_nic_pci_change_crbwindow_128M(adapter, 0);
-         */
+        off8 = off & ~(stride-1);
 
-        for (i = 0; i < loop; i++) {
-                writel(off8 + (i << 3), mem_crb + MIU_TEST_AGT_ADDR_LO);
-                writel(0, mem_crb + MIU_TEST_AGT_ADDR_HI);
-                writel(MIU_TA_CTL_ENABLE, mem_crb + MIU_TEST_AGT_CTRL);
-                writel(MIU_TA_CTL_START | MIU_TA_CTL_ENABLE,
-                                mem_crb + MIU_TEST_AGT_CTRL);
+        spin_lock(&adapter->ahw.mem_lock);
 
-                for (j = 0; j < MAX_CTL_CHECK; j++) {
-                        temp = readl(mem_crb + MIU_TEST_AGT_CTRL);
-                        if ((temp & MIU_TA_CTL_BUSY) == 0)
-                                break;
-                }
+        writel(off8, (mem_crb + MIU_TEST_AGT_ADDR_LO));
+        writel(0, (mem_crb + MIU_TEST_AGT_ADDR_HI));
+        writel(TA_CTL_ENABLE, (mem_crb + TEST_AGT_CTRL));
+        writel((TA_CTL_START | TA_CTL_ENABLE), (mem_crb + TEST_AGT_CTRL));
 
-                if (j >= MAX_CTL_CHECK) {
-                        if (printk_ratelimit())
-                                dev_err(&adapter->pdev->dev,
-                                        "failed to read through agent\n");
+        for (j = 0; j < MAX_CTL_CHECK; j++) {
+                temp = readl(mem_crb + TEST_AGT_CTRL);
+                if ((temp & TA_CTL_BUSY) == 0)
                         break;
-                }
-
-                start = off0[i] >> 2;
-                end   = (off0[i] + sz[i] - 1) >> 2;
-                for (k = start; k <= end; k++) {
-                        temp = readl(mem_crb + MIU_TEST_AGT_RDDATA(k));
-                        word[i] |= ((uint64_t)temp << (32 * k));
-                }
         }
 
-        /*
-         * netxen_nic_pci_change_crbwindow_128M(adapter, 1);
-         * write_unlock_irqrestore(&adapter->adapter_lock, flags);
-         */
-
-        if (j >= MAX_CTL_CHECK)
-                return -1;
-
-        if (sz[0] == 8) {
-                val = word[0];
+        if (j >= MAX_CTL_CHECK) {
+                if (printk_ratelimit())
+                        dev_err(&adapter->pdev->dev,
+                                        "failed to read through agent\n");
+                ret = -EIO;
         } else {
-                val = ((word[0] >> (off0[0] * 8)) & (~(~0ULL << (sz[0] * 8)))) |
-                ((word[1] & (~(~0ULL << (sz[1] * 8)))) << (sz[0] * 8));
-        }
+                off8 = MIU_TEST_AGT_RDDATA_LO;
+                if ((stride == 16) && (off & 0xf))
+                        off8 = MIU_TEST_AGT_RDDATA_UPPER_LO;
 
-        switch (size) {
-        case 1:
-                *(uint8_t  *)data = val;
-                break;
-        case 2:
-                *(uint16_t *)data = val;
-                break;
-        case 4:
-                *(uint32_t *)data = val;
-                break;
-        case 8:
-                *(uint64_t *)data = val;
-                break;
+                temp = readl(mem_crb + off8 + 4);
+                val = (u64)temp << 32;
+                val |= readl(mem_crb + off8);
+                *data = val;
+                ret = 0;
         }
-        return 0;
+
+        spin_unlock(&adapter->ahw.mem_lock);
+
+        return ret;
 }
 
 void
@@ -2037,10 +1937,10 @@ void netxen_nic_set_link_parameters(struct netxen_adapter *adapter)
                         return;
                 }
 
-                if (adapter->phy_read
-                    && adapter->phy_read(adapter,
-                             NETXEN_NIU_GB_MII_MGMT_ADDR_PHY_STATUS,
-                             &status) == 0) {
+                if (adapter->phy_read &&
+                    adapter->phy_read(adapter,
+                                      NETXEN_NIU_GB_MII_MGMT_ADDR_PHY_STATUS,
+                                      &status) == 0) {
                         if (netxen_get_phy_link(status)) {
                                 switch (netxen_get_phy_speed(status)) {
                                 case 0:
@@ -2067,10 +1967,10 @@ void netxen_nic_set_link_parameters(struct netxen_adapter *adapter)
                                         adapter->link_duplex = -1;
                                         break;
                                 }
-                                if (adapter->phy_read
-                                    && adapter->phy_read(adapter,
-                                             NETXEN_NIU_GB_MII_MGMT_ADDR_AUTONEG,
-                                             &autoneg) != 0)
+                                if (adapter->phy_read &&
+                                    adapter->phy_read(adapter,
+                                                      NETXEN_NIU_GB_MII_MGMT_ADDR_AUTONEG,
+                                                      &autoneg) != 0)
                                         adapter->link_autoneg = autoneg;
                         } else
                                 goto link_down;