1 files changed, 468 insertions, 227 deletions

diff --git a/drivers/net/gianfar.c b/drivers/net/gianfar.c
index fa0188ea9233..aa258e899261 100644
--- a/drivers/net/gianfar.c
+++ b/drivers/net/gianfar.c
@@ -143,6 +143,7 @@ void gfar_start(struct net_device *dev);
 static void gfar_clear_exact_match(struct net_device *dev);
 static void gfar_set_mac_for_addr(struct net_device *dev, int num, u8 *addr);
 static int gfar_ioctl(struct net_device *dev, struct ifreq *rq, int cmd);
+u16 gfar_select_queue(struct net_device *dev, struct sk_buff *skb);
 
 MODULE_AUTHOR("Freescale Semiconductor, Inc");
 MODULE_DESCRIPTION("Gianfar Ethernet Driver");
@@ -171,71 +172,89 @@ static int gfar_init_bds(struct net_device *ndev)
         struct gfar_priv_rx_q *rx_queue = NULL;
         struct txbd8 *txbdp;
         struct rxbd8 *rxbdp;
-        int i;
-
-        tx_queue = priv->tx_queue;
-        rx_queue = priv->rx_queue;
+        int i, j;
 
-        /* Initialize some variables in our dev structure */
-        tx_queue->num_txbdfree = tx_queue->tx_ring_size;
-        tx_queue->dirty_tx = tx_queue->cur_tx = tx_queue->tx_bd_base;
-        rx_queue->cur_rx = rx_queue->rx_bd_base;
-        tx_queue->skb_curtx = tx_queue->skb_dirtytx = 0;
-        rx_queue->skb_currx = 0;
+        for (i = 0; i < priv->num_tx_queues; i++) {
+                tx_queue = priv->tx_queue[i];
+                /* Initialize some variables in our dev structure */
+                tx_queue->num_txbdfree = tx_queue->tx_ring_size;
+                tx_queue->dirty_tx = tx_queue->tx_bd_base;
+                tx_queue->cur_tx = tx_queue->tx_bd_base;
+                tx_queue->skb_curtx = 0;
+                tx_queue->skb_dirtytx = 0;
+
+                /* Initialize Transmit Descriptor Ring */
+                txbdp = tx_queue->tx_bd_base;
+                for (j = 0; j < tx_queue->tx_ring_size; j++) {
+                        txbdp->lstatus = 0;
+                        txbdp->bufPtr = 0;
+                        txbdp++;
+                }
 
-        /* Initialize Transmit Descriptor Ring */
-        txbdp = tx_queue->tx_bd_base;
-        for (i = 0; i < tx_queue->tx_ring_size; i++) {
-                txbdp->lstatus = 0;
-                txbdp->bufPtr = 0;
-                txbdp++;
+                /* Set the last descriptor in the ring to indicate wrap */
+                txbdp--;
+                txbdp->status |= TXBD_WRAP;
         }
 
-        /* Set the last descriptor in the ring to indicate wrap */
-        txbdp--;
-        txbdp->status |= TXBD_WRAP;
+        for (i = 0; i < priv->num_rx_queues; i++) {
+                rx_queue = priv->rx_queue[i];
+                rx_queue->cur_rx = rx_queue->rx_bd_base;
+                rx_queue->skb_currx = 0;
+                rxbdp = rx_queue->rx_bd_base;
 
-        rxbdp = rx_queue->rx_bd_base;
-        for (i = 0; i < rx_queue->rx_ring_size; i++) {
-                struct sk_buff *skb = rx_queue->rx_skbuff[i];
+                for (j = 0; j < rx_queue->rx_ring_size; j++) {
+                        struct sk_buff *skb = rx_queue->rx_skbuff[j];
 
-                if (skb) {
-                        gfar_init_rxbdp(rx_queue, rxbdp, rxbdp->bufPtr);
-                } else {
-                        skb = gfar_new_skb(ndev);
-                        if (!skb) {
-                                pr_err("%s: Can't allocate RX buffers\n",
-                                       ndev->name);
-                                return -ENOMEM;
+                        if (skb) {
+                                gfar_init_rxbdp(rx_queue, rxbdp,
+                                                rxbdp->bufPtr);
+                        } else {
+                                skb = gfar_new_skb(ndev);
+                                if (!skb) {
+                                        pr_err("%s: Can't allocate RX buffers\n",
+                                                        ndev->name);
+                                        goto err_rxalloc_fail;
+                                }
+                                rx_queue->rx_skbuff[j] = skb;
+
+                                gfar_new_rxbdp(rx_queue, rxbdp, skb);
                         }
-                        rx_queue->rx_skbuff[i] = skb;
 
-                        gfar_new_rxbdp(rx_queue, rxbdp, skb);
+                        rxbdp++;
                 }
 
-                rxbdp++;
         }
 
         return 0;
+
+err_rxalloc_fail:
+        free_skb_resources(priv);
+        return -ENOMEM;
 }
 
 static int gfar_alloc_skb_resources(struct net_device *ndev)
 {
         void *vaddr;
-        int i;
+        dma_addr_t addr;
+        int i, j, k;
         struct gfar_private *priv = netdev_priv(ndev);
         struct device *dev = &priv->ofdev->dev;
         struct gfar_priv_tx_q *tx_queue = NULL;
         struct gfar_priv_rx_q *rx_queue = NULL;
 
-        tx_queue = priv->tx_queue;
-        rx_queue = priv->rx_queue;
+        priv->total_tx_ring_size = 0;
+        for (i = 0; i < priv->num_tx_queues; i++)
+                priv->total_tx_ring_size += priv->tx_queue[i]->tx_ring_size;
+
+        priv->total_rx_ring_size = 0;
+        for (i = 0; i < priv->num_rx_queues; i++)
+                priv->total_rx_ring_size += priv->rx_queue[i]->rx_ring_size;
 
         /* Allocate memory for the buffer descriptors */
         vaddr = dma_alloc_coherent(dev,
-                        sizeof(*tx_queue->tx_bd_base) * tx_queue->tx_ring_size +
-                        sizeof(*rx_queue->rx_bd_base) * rx_queue->rx_ring_size,
-                        &tx_queue->tx_bd_dma_base, GFP_KERNEL);
+                        sizeof(struct txbd8) * priv->total_tx_ring_size +
+                        sizeof(struct rxbd8) * priv->total_rx_ring_size,
+                        &addr, GFP_KERNEL);
         if (!vaddr) {
                 if (netif_msg_ifup(priv))
                         pr_err("%s: Could not allocate buffer descriptors!\n",
@@ -243,38 +262,57 @@ static int gfar_alloc_skb_resources(struct net_device *ndev)
                 return -ENOMEM;
         }
 
-        tx_queue->tx_bd_base = vaddr;
-        tx_queue->dev = ndev;
+        for (i = 0; i < priv->num_tx_queues; i++) {
+                tx_queue = priv->tx_queue[i];
+                tx_queue->tx_bd_base = (struct txbd8 *) vaddr;
+                tx_queue->tx_bd_dma_base = addr;
+                tx_queue->dev = ndev;
+                /* enet DMA only understands physical addresses */
+                addr    += sizeof(struct txbd8) *tx_queue->tx_ring_size;
+                vaddr   += sizeof(struct txbd8) *tx_queue->tx_ring_size;
+        }
 
         /* Start the rx descriptor ring where the tx ring leaves off */
-        vaddr = vaddr + sizeof(*tx_queue->tx_bd_base) * tx_queue->tx_ring_size;
-        rx_queue->rx_bd_base = vaddr;
-        rx_queue->dev = ndev;
+        for (i = 0; i < priv->num_rx_queues; i++) {
+                rx_queue = priv->rx_queue[i];
+                rx_queue->rx_bd_base = (struct rxbd8 *) vaddr;
+                rx_queue->rx_bd_dma_base = addr;
+                rx_queue->dev = ndev;
+                addr    += sizeof (struct rxbd8) * rx_queue->rx_ring_size;
+                vaddr   += sizeof (struct rxbd8) * rx_queue->rx_ring_size;
+        }
 
         /* Setup the skbuff rings */
-        tx_queue->tx_skbuff = kmalloc(sizeof(*tx_queue->tx_skbuff) *
+        for (i = 0; i < priv->num_tx_queues; i++) {
+                tx_queue = priv->tx_queue[i];
+                tx_queue->tx_skbuff = kmalloc(sizeof(*tx_queue->tx_skbuff) *
                                   tx_queue->tx_ring_size, GFP_KERNEL);
-        if (!tx_queue->tx_skbuff) {
-                if (netif_msg_ifup(priv))
-                        pr_err("%s: Could not allocate tx_skbuff\n",
-                               ndev->name);
-                goto cleanup;
-        }
+                if (!tx_queue->tx_skbuff) {
+                        if (netif_msg_ifup(priv))
+                                pr_err("%s: Could not allocate tx_skbuff\n",
+                                                ndev->name);
+                        goto cleanup;
+                }
 
-        for (i = 0; i < tx_queue->tx_ring_size; i++)
-                tx_queue->tx_skbuff[i] = NULL;
+                for (k = 0; k < tx_queue->tx_ring_size; k++)
+                        tx_queue->tx_skbuff[k] = NULL;
+        }
 
-        rx_queue->rx_skbuff = kmalloc(sizeof(*rx_queue->rx_skbuff) *
+        for (i = 0; i < priv->num_rx_queues; i++) {
+                rx_queue = priv->rx_queue[i];
+                rx_queue->rx_skbuff = kmalloc(sizeof(*rx_queue->rx_skbuff) *
                                   rx_queue->rx_ring_size, GFP_KERNEL);
-        if (!rx_queue->rx_skbuff) {
-                if (netif_msg_ifup(priv))
-                        pr_err("%s: Could not allocate rx_skbuff\n",
-                               ndev->name);
-                goto cleanup;
-        }
 
-        for (i = 0; i < rx_queue->rx_ring_size; i++)
-                rx_queue->rx_skbuff[i] = NULL;
+                if (!rx_queue->rx_skbuff) {
+                        if (netif_msg_ifup(priv))
+                                pr_err("%s: Could not allocate rx_skbuff\n",
+                                       ndev->name);
+                        goto cleanup;
+                }
+
+                for (j = 0; j < rx_queue->rx_ring_size; j++)
+                        rx_queue->rx_skbuff[j] = NULL;
+        }
 
         if (gfar_init_bds(ndev))
                 goto cleanup;
@@ -286,33 +324,47 @@ cleanup:
         return -ENOMEM;
 }
 
+static void gfar_init_tx_rx_base(struct gfar_private *priv)
+{
+        struct gfar __iomem *regs = priv->gfargrp.regs;
+        u32 *baddr;
+        int i;
+
+        baddr = &regs->tbase0;
+        for(i = 0; i < priv->num_tx_queues; i++) {
+                gfar_write(baddr, priv->tx_queue[i]->tx_bd_dma_base);
+                baddr   += 2;
+        }
+
+        baddr = &regs->rbase0;
+        for(i = 0; i < priv->num_rx_queues; i++) {
+                gfar_write(baddr, priv->rx_queue[i]->rx_bd_dma_base);
+                baddr   += 2;
+        }
+}
+
 static void gfar_init_mac(struct net_device *ndev)
 {
         struct gfar_private *priv = netdev_priv(ndev);
-        struct gfar_priv_tx_q *tx_queue = NULL;
-        struct gfar_priv_rx_q *rx_queue = NULL;
         struct gfar __iomem *regs = priv->gfargrp.regs;
         u32 rctrl = 0;
         u32 tctrl = 0;
         u32 attrs = 0;
 
-        tx_queue = priv->tx_queue;
-        rx_queue = priv->rx_queue;
-
-        /* enet DMA only understands physical addresses */
-        gfar_write(&regs->tbase0, tx_queue->tx_bd_dma_base);
-        gfar_write(&regs->rbase0, tx_queue->tx_bd_dma_base +
-                                  sizeof(*tx_queue->tx_bd_base) *
-                                  tx_queue->tx_ring_size);
+        /* write the tx/rx base registers */
+        gfar_init_tx_rx_base(priv);
 
         /* Configure the coalescing support */
         gfar_write(&regs->txic, 0);
-        if (tx_queue->txcoalescing)
-                gfar_write(&regs->txic, tx_queue->txic);
+        if (priv->tx_queue[0]->txcoalescing)
+                gfar_write(&regs->txic, priv->tx_queue[0]->txic);
 
         gfar_write(&regs->rxic, 0);
-        if (rx_queue->rxcoalescing)
-                gfar_write(&regs->rxic, rx_queue->rxic);
+        if (priv->rx_queue[0]->rxcoalescing)
+                gfar_write(&regs->rxic, priv->rx_queue[0]->rxic);
+
+        if (priv->rx_filer_enable)
+                rctrl |= RCTRL_FILREN;
 
         if (priv->rx_csum_enable)
                 rctrl |= RCTRL_CHECKSUMMING;
@@ -341,6 +393,8 @@ static void gfar_init_mac(struct net_device *ndev)
         if (ndev->features & NETIF_F_IP_CSUM)
                 tctrl |= TCTRL_INIT_CSUM;
 
+        tctrl |= TCTRL_TXSCHED_PRIO;
+
         gfar_write(&regs->tctrl, tctrl);
 
         /* Set the extraction length and index */
@@ -374,6 +428,7 @@ static const struct net_device_ops gfar_netdev_ops = {
         .ndo_set_multicast_list = gfar_set_multi,
         .ndo_tx_timeout = gfar_timeout,
         .ndo_do_ioctl = gfar_ioctl,
+        .ndo_select_queue = gfar_select_queue,
         .ndo_vlan_rx_register = gfar_vlan_rx_register,
         .ndo_set_mac_address = eth_mac_addr,
         .ndo_validate_addr = eth_validate_addr,
@@ -382,36 +437,131 @@ static const struct net_device_ops gfar_netdev_ops = {
 #endif
 };
 
+void lock_rx_qs(struct gfar_private *priv)
+{
+        int i = 0x0;
+
+        for (i = 0; i < priv->num_rx_queues; i++)
+                spin_lock(&priv->rx_queue[i]->rxlock);
+}
+
+void lock_tx_qs(struct gfar_private *priv)
+{
+        int i = 0x0;
+
+        for (i = 0; i < priv->num_tx_queues; i++)
+                spin_lock(&priv->tx_queue[i]->txlock);
+}
+
+void unlock_rx_qs(struct gfar_private *priv)
+{
+        int i = 0x0;
+
+        for (i = 0; i < priv->num_rx_queues; i++)
+                spin_unlock(&priv->rx_queue[i]->rxlock);
+}
+
+void unlock_tx_qs(struct gfar_private *priv)
+{
+        int i = 0x0;
+
+        for (i = 0; i < priv->num_tx_queues; i++)
+                spin_unlock(&priv->tx_queue[i]->txlock);
+}
+
 /* Returns 1 if incoming frames use an FCB */
 static inline int gfar_uses_fcb(struct gfar_private *priv)
 {
         return priv->vlgrp || priv->rx_csum_enable;
 }
 
-static int gfar_of_init(struct net_device *dev)
+u16 gfar_select_queue(struct net_device *dev, struct sk_buff *skb)
+{
+        return skb_get_queue_mapping(skb);
+}
+static void free_tx_pointers(struct gfar_private *priv)
+{
+        int i = 0;
+
+        for (i = 0; i < priv->num_tx_queues; i++)
+                kfree(priv->tx_queue[i]);
+}
+
+static void free_rx_pointers(struct gfar_private *priv)
+{
+        int i = 0;
+
+        for (i = 0; i < priv->num_rx_queues; i++)
+                kfree(priv->rx_queue[i]);
+}
+
+static int gfar_of_init(struct of_device *ofdev, struct net_device **pdev)
 {
         const char *model;
         const char *ctype;
         const void *mac_addr;
         u64 addr, size;
-        int err = 0;
-        struct gfar_private *priv = netdev_priv(dev);
-        struct device_node *np = priv->node;
+        int err = 0, i;
+        struct net_device *dev = NULL;
+        struct gfar_private *priv = NULL;
+        struct device_node *np = ofdev->node;
         const u32 *stash;
         const u32 *stash_len;
         const u32 *stash_idx;
+        unsigned int num_tx_qs, num_rx_qs;
+        u32 *tx_queues, *rx_queues;
 
         if (!np || !of_device_is_available(np))
                 return -ENODEV;
 
+        /* parse the num of tx and rx queues */
+        tx_queues = (u32 *)of_get_property(np, "fsl,num_tx_queues", NULL);
+        num_tx_qs = tx_queues ? *tx_queues : 1;
+
+        if (num_tx_qs > MAX_TX_QS) {
+                printk(KERN_ERR "num_tx_qs(=%d) greater than MAX_TX_QS(=%d)\n",
+                                num_tx_qs, MAX_TX_QS);
+                printk(KERN_ERR "Cannot do alloc_etherdev, aborting\n");
+                return -EINVAL;
+        }
+
+        rx_queues = (u32 *)of_get_property(np, "fsl,num_rx_queues", NULL);
+        num_rx_qs = rx_queues ? *rx_queues : 1;
+
+        if (num_rx_qs > MAX_RX_QS) {
+                printk(KERN_ERR "num_rx_qs(=%d) greater than MAX_RX_QS(=%d)\n",
+                                num_tx_qs, MAX_TX_QS);
+                printk(KERN_ERR "Cannot do alloc_etherdev, aborting\n");
+                return -EINVAL;
+        }
+
+        *pdev = alloc_etherdev_mq(sizeof(*priv), num_tx_qs);
+        dev = *pdev;
+        if (NULL == dev)
+                return -ENOMEM;
+
+        priv = netdev_priv(dev);
+        priv->node = ofdev->node;
+        priv->ndev = dev;
+
+        dev->num_tx_queues = num_tx_qs;
+        dev->real_num_tx_queues = num_tx_qs;
+        priv->num_tx_queues = num_tx_qs;
+        priv->num_rx_queues = num_rx_qs;
+
         /* get a pointer to the register memory */
         addr = of_translate_address(np, of_get_address(np, 0, &size, NULL));
         priv->gfargrp.regs = ioremap(addr, size);
 
-        if (priv->gfargrp.regs == NULL)
-                return -ENOMEM;
+        if (priv->gfargrp.regs == NULL) {
+                err = -ENOMEM;
+                goto err_out;
+        }
 
         priv->gfargrp.priv = priv; /* back pointer from group to priv */
+        priv->gfargrp.rx_bit_map = DEFAULT_MAPPING;
+        priv->gfargrp.tx_bit_map = DEFAULT_MAPPING;
+
         priv->gfargrp.interruptTransmit = irq_of_parse_and_map(np, 0);
 
         model = of_get_property(np, "model", NULL);
@@ -430,6 +580,38 @@ static int gfar_of_init(struct net_device *dev)
                 }
         }
 
+        for (i = 0; i < priv->num_tx_queues; i++)
+               priv->tx_queue[i] = NULL;
+        for (i = 0; i < priv->num_rx_queues; i++)
+                priv->rx_queue[i] = NULL;
+
+        for (i = 0; i < priv->num_tx_queues; i++) {
+                priv->tx_queue[i] =  (struct gfar_priv_tx_q *)kmalloc(
+                                sizeof (struct gfar_priv_tx_q), GFP_KERNEL);
+                if (!priv->tx_queue[i]) {
+                        err = -ENOMEM;
+                        goto tx_alloc_failed;
+                }
+                priv->tx_queue[i]->tx_skbuff = NULL;
+                priv->tx_queue[i]->qindex = i;
+                priv->tx_queue[i]->dev = dev;
+                spin_lock_init(&(priv->tx_queue[i]->txlock));
+        }
+
+        for (i = 0; i < priv->num_rx_queues; i++) {
+                priv->rx_queue[i] = (struct gfar_priv_rx_q *)kmalloc(
+                                        sizeof (struct gfar_priv_rx_q), GFP_KERNEL);
+                if (!priv->rx_queue[i]) {
+                        err = -ENOMEM;
+                        goto rx_alloc_failed;
+                }
+                priv->rx_queue[i]->rx_skbuff = NULL;
+                priv->rx_queue[i]->qindex = i;
+                priv->rx_queue[i]->dev = dev;
+                spin_lock_init(&(priv->rx_queue[i]->rxlock));
+        }
+
+
         stash = of_get_property(np, "bd-stash", NULL);
 
         if (stash) {
@@ -490,8 +672,13 @@ static int gfar_of_init(struct net_device *dev)
 
         return 0;
 
+rx_alloc_failed:
+        free_rx_pointers(priv);
+tx_alloc_failed:
+        free_tx_pointers(priv);
 err_out:
         iounmap(priv->gfargrp.regs);
+        free_netdev(dev);
         return err;
 }
 
@@ -509,6 +696,17 @@ static int gfar_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
         return phy_mii_ioctl(priv->phydev, if_mii(rq), cmd);
 }
 
+static unsigned int reverse_bitmap(unsigned int bit_map, unsigned int max_qs)
+{
+        unsigned int new_bit_map = 0x0;
+        int mask = 0x1 << (max_qs - 1), i;
+        for (i = 0; i < max_qs; i++) {
+                if (bit_map & mask)
+                        new_bit_map = new_bit_map + (1 << i);
+                mask = mask >> 0x1;
+        }
+        return new_bit_map;
+}
 /* Set up the ethernet device structure, private data,
  * and anything else we need before we start */
 static int gfar_probe(struct of_device *ofdev,
@@ -518,14 +716,14 @@ static int gfar_probe(struct of_device *ofdev,
         struct net_device *dev = NULL;
         struct gfar_private *priv = NULL;
         struct gfar __iomem *regs = NULL;
-        int err = 0;
+        int err = 0, i;
         int len_devname;
+        u32 rstat = 0, tstat = 0, rqueue = 0, tqueue = 0;
 
-        /* Create an ethernet device instance */
-        dev = alloc_etherdev(sizeof (*priv));
+        err = gfar_of_init(ofdev, &dev);
 
-        if (NULL == dev)
-                return -ENOMEM;
+        if (err)
+                return err;
 
         priv = netdev_priv(dev);
         priv->ndev = dev;
@@ -533,23 +731,6 @@ static int gfar_probe(struct of_device *ofdev,
         priv->node = ofdev->node;
         SET_NETDEV_DEV(dev, &ofdev->dev);
 
-        err = gfar_of_init(dev);
-
-        if (err)
-                goto regs_fail;
-
-        priv->tx_queue = (struct gfar_priv_tx_q *)kmalloc(
-                                sizeof (struct gfar_priv_tx_q), GFP_KERNEL);
-        if (!priv->tx_queue)
-                goto regs_fail;
-
-        priv->rx_queue = (struct gfar_priv_rx_q *)kmalloc(
-                                sizeof (struct gfar_priv_rx_q), GFP_KERNEL);
-        if (!priv->rx_queue)
-                goto rx_queue_fail;
-
-        spin_lock_init(&priv->tx_queue->txlock);
-        spin_lock_init(&priv->rx_queue->rxlock);
         spin_lock_init(&priv->gfargrp.grplock);
         spin_lock_init(&priv->bflock);
         INIT_WORK(&priv->reset_task, gfar_reset_task);
@@ -587,8 +768,8 @@ static int gfar_probe(struct of_device *ofdev,
         dev->netdev_ops = &gfar_netdev_ops;
         dev->ethtool_ops = &gfar_ethtool_ops;
 
-        /* Register for napi ...NAPI is for each rx_queue */
-        netif_napi_add(dev, &priv->rx_queue->napi, gfar_poll, GFAR_DEV_WEIGHT);
+        /* Register for napi ...We are registering NAPI for each grp */
+        netif_napi_add(dev, &priv->gfargrp.napi, gfar_poll, GFAR_DEV_WEIGHT);
 
         if (priv->device_flags & FSL_GIANFAR_DEV_HAS_CSUM) {
                 priv->rx_csum_enable = 1;
@@ -644,17 +825,44 @@ static int gfar_probe(struct of_device *ofdev,
         if (dev->features & NETIF_F_IP_CSUM)
                 dev->hard_header_len += GMAC_FCB_LEN;
 
+        /* Need to reverse the bit maps as  bit_map's MSB is q0
+         * but, for_each_bit parses from right to left, which
+         * basically reverses the queue numbers */
+        priv->gfargrp.tx_bit_map = reverse_bitmap(priv->gfargrp.tx_bit_map, MAX_TX_QS);
+        priv->gfargrp.rx_bit_map = reverse_bitmap(priv->gfargrp.rx_bit_map, MAX_RX_QS);
+
+        /* Calculate RSTAT, TSTAT, RQUEUE and TQUEUE values */
+        for_each_bit(i, &priv->gfargrp.rx_bit_map, priv->num_rx_queues) {
+                priv->gfargrp.num_rx_queues++;
+                rstat = rstat | (RSTAT_CLEAR_RHALT >> i);
+                rqueue = rqueue | ((RQUEUE_EN0 | RQUEUE_EX0) >> i);
+        }
+        for_each_bit (i, &priv->gfargrp.tx_bit_map, priv->num_tx_queues) {
+                priv->gfargrp.num_tx_queues++;
+                tstat = tstat | (TSTAT_CLEAR_THALT >> i);
+                tqueue = tqueue | (TQUEUE_EN0 >> i);
+        }
+        priv->gfargrp.rstat = rstat;
+        priv->gfargrp.tstat = tstat;
+
+        gfar_write(&regs->rqueue, rqueue);
+        gfar_write(&regs->tqueue, tqueue);
+
         priv->rx_buffer_size = DEFAULT_RX_BUFFER_SIZE;
 
         /* Initializing some of the rx/tx queue level parameters */
-        priv->tx_queue->tx_ring_size = DEFAULT_TX_RING_SIZE;
-        priv->tx_queue->num_txbdfree = DEFAULT_TX_RING_SIZE;
-        priv->tx_queue->txcoalescing = DEFAULT_TX_COALESCE;
-        priv->tx_queue->txic = DEFAULT_TXIC;
+        for (i = 0; i < priv->num_tx_queues; i++) {
+                priv->tx_queue[i]->tx_ring_size = DEFAULT_TX_RING_SIZE;
+                priv->tx_queue[i]->num_txbdfree = DEFAULT_TX_RING_SIZE;
+                priv->tx_queue[i]->txcoalescing = DEFAULT_TX_COALESCE;
+                priv->tx_queue[i]->txic = DEFAULT_TXIC;
+        }
 
-        priv->rx_queue->rx_ring_size = DEFAULT_RX_RING_SIZE;
-        priv->rx_queue->rxcoalescing = DEFAULT_RX_COALESCE;
-        priv->rx_queue->rxic = DEFAULT_RXIC;
+        for (i = 0; i < priv->num_rx_queues; i++) {
+                priv->rx_queue[i]->rx_ring_size = DEFAULT_RX_RING_SIZE;
+                priv->rx_queue[i]->rxcoalescing = DEFAULT_RX_COALESCE;
+                priv->rx_queue[i]->rxic = DEFAULT_RXIC;
+        }
 
         /* Enable most messages by default */
         priv->msg_enable = (NETIF_MSG_IFUP << 1 ) - 1;
@@ -699,17 +907,19 @@ static int gfar_probe(struct of_device *ofdev,
         /* Even more device info helps when determining which kernel */
         /* provided which set of benchmarks. */
         printk(KERN_INFO "%s: Running with NAPI enabled\n", dev->name);
-        printk(KERN_INFO "%s: %d/%d RX/TX BD ring size\n",
-               dev->name, priv->rx_queue->rx_ring_size, priv->tx_queue->tx_ring_size);
+        for (i = 0; i < priv->num_rx_queues; i++)
+                printk(KERN_INFO "%s: :RX BD ring size for Q[%d]: %d\n",
+                        dev->name, i, priv->rx_queue[i]->rx_ring_size);
+        for(i = 0; i < priv->num_tx_queues; i++)
+                 printk(KERN_INFO "%s:TX BD ring size for Q[%d]: %d\n",
+                        dev->name, i, priv->tx_queue[i]->tx_ring_size);
 
         return 0;
 
 register_fail:
         iounmap(priv->gfargrp.regs);
-        kfree(priv->rx_queue);
-rx_queue_fail:
-        kfree(priv->tx_queue);
-regs_fail:
+        free_tx_pointers(priv);
+        free_rx_pointers(priv);
         if (priv->phy_node)
                 of_node_put(priv->phy_node);
         if (priv->tbi_node)
@@ -742,8 +952,6 @@ static int gfar_suspend(struct device *dev)
 {
         struct gfar_private *priv = dev_get_drvdata(dev);
         struct net_device *ndev = priv->ndev;
-        struct gfar_priv_tx_q *tx_queue = NULL;
-        struct gfar_priv_rx_q *rx_queue = NULL;
         struct gfar __iomem *regs = NULL;
         unsigned long flags;
         u32 tempval;
@@ -752,13 +960,13 @@ static int gfar_suspend(struct device *dev)
                 (priv->device_flags & FSL_GIANFAR_DEV_HAS_MAGIC_PACKET);
 
         netif_device_detach(ndev);
-        tx_queue = priv->tx_queue;
-        rx_queue = priv->rx_queue;
         regs = priv->gfargrp.regs;
 
         if (netif_running(ndev)) {
-                spin_lock_irqsave(&tx_queue->txlock, flags);
-                spin_lock(&rx_queue->rxlock);
+
+                local_irq_save(flags);
+                lock_tx_qs(priv);
+                lock_rx_qs(priv);
 
                 gfar_halt_nodisable(ndev);
 
@@ -772,10 +980,11 @@ static int gfar_suspend(struct device *dev)
 
                 gfar_write(&regs->maccfg1, tempval);
 
-                spin_unlock(&rx_queue->rxlock);
-                spin_unlock_irqrestore(&tx_queue->txlock, flags);
+                unlock_rx_qs(priv);
+                unlock_tx_qs(priv);
+                local_irq_restore(flags);
 
-                napi_disable(&rx_queue->napi);
+                napi_disable(&priv->gfargrp.napi);
 
                 if (magic_packet) {
                         /* Enable interrupt on Magic Packet */
@@ -797,8 +1006,6 @@ static int gfar_resume(struct device *dev)
 {
         struct gfar_private *priv = dev_get_drvdata(dev);
         struct net_device *ndev = priv->ndev;
-        struct gfar_priv_tx_q *tx_queue = NULL;
-        struct gfar_priv_rx_q *rx_queue = NULL;
         struct gfar __iomem *regs = NULL;
         unsigned long flags;
         u32 tempval;
@@ -816,12 +1023,11 @@ static int gfar_resume(struct device *dev)
         /* Disable Magic Packet mode, in case something
          * else woke us up.
          */
-        rx_queue = priv->rx_queue;
-        tx_queue = priv->tx_queue;
         regs = priv->gfargrp.regs;
 
-        spin_lock_irqsave(&tx_queue->txlock, flags);
-        spin_lock(&rx_queue->rxlock);
+        local_irq_save(flags);
+        lock_tx_qs(priv);
+        lock_rx_qs(priv);
 
         tempval = gfar_read(&regs->maccfg2);
         tempval &= ~MACCFG2_MPEN;
@@ -829,12 +1035,13 @@ static int gfar_resume(struct device *dev)
 
         gfar_start(ndev);
 
-        spin_unlock(&rx_queue->rxlock);
-        spin_unlock_irqrestore(&tx_queue->txlock, flags);
+        unlock_rx_qs(priv);
+        unlock_tx_qs(priv);
+        local_irq_restore(flags);
 
         netif_device_attach(ndev);
 
-        napi_enable(&rx_queue->napi);
+        napi_enable(&priv->gfargrp.napi);
 
         return 0;
 }
@@ -861,7 +1068,7 @@ static int gfar_restore(struct device *dev)
                 phy_start(priv->phydev);
 
         netif_device_attach(ndev);
-        napi_enable(&priv->napi);
+        napi_enable(&priv->gfargrp.napi);
 
         return 0;
 }
@@ -1115,23 +1322,21 @@ void gfar_halt(struct net_device *dev)
 void stop_gfar(struct net_device *dev)
 {
         struct gfar_private *priv = netdev_priv(dev);
-        struct gfar_priv_tx_q *tx_queue = NULL;
-        struct gfar_priv_rx_q *rx_queue = NULL;
         unsigned long flags;
 
         phy_stop(priv->phydev);
 
-        tx_queue = priv->tx_queue;
-        rx_queue = priv->rx_queue;
 
         /* Lock it down */
-        spin_lock_irqsave(&tx_queue->txlock, flags);
-        spin_lock(&rx_queue->rxlock);
+        local_irq_save(flags);
+        lock_tx_qs(priv);
+        lock_rx_qs(priv);
 
         gfar_halt(dev);
 
-        spin_unlock(&rx_queue->rxlock);
-        spin_unlock_irqrestore(&tx_queue->txlock, flags);
+        unlock_rx_qs(priv);
+        unlock_tx_qs(priv);
+        local_irq_restore(flags);
 
         /* Free the IRQs */
         if (priv->device_flags & FSL_GIANFAR_DEV_HAS_MULTI_INTR) {
@@ -1145,24 +1350,14 @@ void stop_gfar(struct net_device *dev)
         free_skb_resources(priv);
 }
 
-/* If there are any tx skbs or rx skbs still around, free them.
- * Then free tx_skbuff and rx_skbuff */
-static void free_skb_resources(struct gfar_private *priv)
+static void free_skb_tx_queue(struct gfar_priv_tx_q *tx_queue)
 {
-        struct device *dev = &priv->ofdev->dev;
-        struct rxbd8 *rxbdp;
         struct txbd8 *txbdp;
-        struct gfar_priv_tx_q *tx_queue = NULL;
-        struct gfar_priv_rx_q *rx_queue = NULL;
+        struct gfar_private *priv = netdev_priv(tx_queue->dev);
         int i, j;
 
-        /* Go through all the buffer descriptors and free their data buffers */
-        tx_queue = priv->tx_queue;
         txbdp = tx_queue->tx_bd_base;
 
-        if (!tx_queue->tx_skbuff)
-                goto skip_tx_skbuff;
-
         for (i = 0; i < tx_queue->tx_ring_size; i++) {
                 if (!tx_queue->tx_skbuff[i])
                         continue;
@@ -1170,7 +1365,8 @@ static void free_skb_resources(struct gfar_private *priv)
                 dma_unmap_single(&priv->ofdev->dev, txbdp->bufPtr,
                                 txbdp->length, DMA_TO_DEVICE);
                 txbdp->lstatus = 0;
-                for (j = 0; j < skb_shinfo(tx_queue->tx_skbuff[i])->nr_frags; j++) {
+                for (j = 0; j < skb_shinfo(tx_queue->tx_skbuff[i])->nr_frags;
+                                j++) {
                         txbdp++;
                         dma_unmap_page(&priv->ofdev->dev, txbdp->bufPtr,
                                         txbdp->length, DMA_TO_DEVICE);
@@ -1179,36 +1375,58 @@ static void free_skb_resources(struct gfar_private *priv)
                 dev_kfree_skb_any(tx_queue->tx_skbuff[i]);
                 tx_queue->tx_skbuff[i] = NULL;
         }
-
         kfree(tx_queue->tx_skbuff);
-skip_tx_skbuff:
+}
 
-        rx_queue = priv->rx_queue;
-        rxbdp = rx_queue->rx_bd_base;
+static void free_skb_rx_queue(struct gfar_priv_rx_q *rx_queue)
+{
+        struct rxbd8 *rxbdp;
+        struct gfar_private *priv = netdev_priv(rx_queue->dev);
+        int i;
 
-        if (!rx_queue->rx_skbuff)
-                goto skip_rx_skbuff;
+        rxbdp = rx_queue->rx_bd_base;
 
         for (i = 0; i < rx_queue->rx_ring_size; i++) {
                 if (rx_queue->rx_skbuff[i]) {
-                        dma_unmap_single(&priv->ofdev->dev, rxbdp->bufPtr,
-                                         priv->rx_buffer_size,
+                        dma_unmap_single(&priv->ofdev->dev,
+                                        rxbdp->bufPtr, priv->rx_buffer_size,
                                         DMA_FROM_DEVICE);
                         dev_kfree_skb_any(rx_queue->rx_skbuff[i]);
                         rx_queue->rx_skbuff[i] = NULL;
                 }
-
                 rxbdp->lstatus = 0;
                 rxbdp->bufPtr = 0;
                 rxbdp++;
         }
-
         kfree(rx_queue->rx_skbuff);
-skip_rx_skbuff:
+}
 
-        dma_free_coherent(dev, sizeof(*txbdp) * tx_queue->tx_ring_size +
-                               sizeof(*rxbdp) * rx_queue->rx_ring_size,
-                          tx_queue->tx_bd_base, tx_queue->tx_bd_dma_base);
+/* If there are any tx skbs or rx skbs still around, free them.
+ * Then free tx_skbuff and rx_skbuff */
+static void free_skb_resources(struct gfar_private *priv)
+{
+        struct gfar_priv_tx_q *tx_queue = NULL;
+        struct gfar_priv_rx_q *rx_queue = NULL;
+        int i;
+
+        /* Go through all the buffer descriptors and free their data buffers */
+        for (i = 0; i < priv->num_tx_queues; i++) {
+                tx_queue = priv->tx_queue[i];
+                if(!tx_queue->tx_skbuff)
+                        free_skb_tx_queue(tx_queue);
+        }
+
+        for (i = 0; i < priv->num_rx_queues; i++) {
+                rx_queue = priv->rx_queue[i];
+                if(!rx_queue->rx_skbuff)
+                        free_skb_rx_queue(rx_queue);
+        }
+
+        dma_free_coherent(&priv->ofdev->dev,
+                        sizeof(struct txbd8) * priv->total_tx_ring_size +
+                        sizeof(struct rxbd8) * priv->total_rx_ring_size,
+                        priv->tx_queue[0]->tx_bd_base,
+                        priv->tx_queue[0]->tx_bd_dma_base);
 }
 
 void gfar_start(struct net_device *dev)
@@ -1233,8 +1451,8 @@ void gfar_start(struct net_device *dev)
         gfar_write(&regs->dmactrl, tempval);
 
         /* Clear THLT/RHLT, so that the DMA starts polling now */
-        gfar_write(&regs->tstat, TSTAT_CLEAR_THALT);
-        gfar_write(&regs->rstat, RSTAT_CLEAR_RHALT);
+        gfar_write(&regs->tstat, priv->gfargrp.tstat);
+        gfar_write(&regs->rstat, priv->gfargrp.rstat);
 
         /* Unmask the interrupts we look for */
         gfar_write(&regs->imask, IMASK_DEFAULT);
@@ -1329,7 +1547,7 @@ static int gfar_enet_open(struct net_device *dev)
         struct gfar_private *priv = netdev_priv(dev);
         int err;
 
-        napi_enable(&priv->rx_queue->napi);
+        napi_enable(&priv->gfargrp.napi);
 
         skb_queue_head_init(&priv->rx_recycle);
 
@@ -1341,17 +1559,17 @@ static int gfar_enet_open(struct net_device *dev)
         err = init_phy(dev);
 
         if (err) {
-                napi_disable(&priv->rx_queue->napi);
+                napi_disable(&priv->gfargrp.napi);
                 return err;
         }
 
         err = startup_gfar(dev);
         if (err) {
-                napi_disable(&priv->rx_queue->napi);
+                napi_disable(&priv->gfargrp.napi);
                 return err;
         }
 
-        netif_start_queue(dev);
+        netif_tx_start_all_queues(dev);
 
         device_set_wakeup_enable(&dev->dev, priv->wol_en);
 
@@ -1421,16 +1639,20 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
         struct gfar_private *priv = netdev_priv(dev);
         struct gfar_priv_tx_q *tx_queue = NULL;
+        struct netdev_queue *txq;
         struct gfar __iomem *regs = NULL;
         struct txfcb *fcb = NULL;
         struct txbd8 *txbdp, *txbdp_start, *base;
         u32 lstatus;
-        int i;
+        int i, rq = 0;
         u32 bufaddr;
         unsigned long flags;
         unsigned int nr_frags, length;
 
-        tx_queue = priv->tx_queue;
+
+        rq = skb->queue_mapping;
+        tx_queue = priv->tx_queue[rq];
+        txq = netdev_get_tx_queue(dev, rq);
         base = tx_queue->tx_bd_base;
         regs = priv->gfargrp.regs;
 
@@ -1458,7 +1680,7 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
         /* check if there is space to queue this packet */
         if ((nr_frags+1) > tx_queue->num_txbdfree) {
                 /* no space, stop the queue */
-                netif_stop_queue(dev);
+                netif_tx_stop_queue(txq);
                 dev->stats.tx_fifo_errors++;
                 spin_unlock_irqrestore(&tx_queue->txlock, flags);
                 return NETDEV_TX_BUSY;
@@ -1550,13 +1772,13 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
         /* If the next BD still needs to be cleaned up, then the bds
            are full.  We need to tell the kernel to stop sending us stuff. */
         if (!tx_queue->num_txbdfree) {
-                netif_stop_queue(dev);
+                netif_tx_stop_queue(txq);
 
                 dev->stats.tx_fifo_errors++;
         }
 
         /* Tell the DMA to go go go */
-        gfar_write(&regs->tstat, TSTAT_CLEAR_THALT);
+        gfar_write(&regs->tstat, TSTAT_CLEAR_THALT >> tx_queue->qindex);
 
         /* Unlock priv */
         spin_unlock_irqrestore(&tx_queue->txlock, flags);
@@ -1569,7 +1791,7 @@ static int gfar_close(struct net_device *dev)
 {
         struct gfar_private *priv = netdev_priv(dev);
 
-        napi_disable(&priv->rx_queue->napi);
+        napi_disable(&priv->gfargrp.napi);
 
         skb_queue_purge(&priv->rx_recycle);
         cancel_work_sync(&priv->reset_task);
@@ -1579,7 +1801,7 @@ static int gfar_close(struct net_device *dev)
         phy_disconnect(priv->phydev);
         priv->phydev = NULL;
 
-        netif_stop_queue(dev);
+        netif_tx_stop_all_queues(dev);
 
         return 0;
 }
@@ -1598,14 +1820,13 @@ static void gfar_vlan_rx_register(struct net_device *dev,
                 struct vlan_group *grp)
 {
         struct gfar_private *priv = netdev_priv(dev);
-        struct gfar_priv_rx_q *rx_queue = NULL;
         struct gfar __iomem *regs = NULL;
         unsigned long flags;
         u32 tempval;
 
-        rx_queue = priv->rx_queue;
         regs = priv->gfargrp.regs;
-        spin_lock_irqsave(&rx_queue->rxlock, flags);
+        local_irq_save(flags);
+        lock_rx_qs(priv);
 
         priv->vlgrp = grp;
 
@@ -1639,7 +1860,8 @@ static void gfar_vlan_rx_register(struct net_device *dev,
 
         gfar_change_mtu(dev, dev->mtu);
 
-        spin_unlock_irqrestore(&rx_queue->rxlock, flags);
+        unlock_rx_qs(priv);
+        local_irq_restore(flags);
 }
 
 static int gfar_change_mtu(struct net_device *dev, int new_mtu)
@@ -1711,10 +1933,10 @@ static void gfar_reset_task(struct work_struct *work)
         struct net_device *dev = priv->ndev;
 
         if (dev->flags & IFF_UP) {
-                netif_stop_queue(dev);
+                netif_tx_stop_all_queues(dev);
                 stop_gfar(dev);
                 startup_gfar(dev);
-                netif_start_queue(dev);
+                netif_tx_start_all_queues(dev);
         }
 
         netif_tx_schedule_all(dev);
@@ -1745,7 +1967,7 @@ static int gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue)
         int howmany = 0;
         u32 lstatus;
 
-        rx_queue = priv->rx_queue;
+        rx_queue = priv->rx_queue[tx_queue->qindex];
         bdp = tx_queue->dirty_tx;
         skb_dirtytx = tx_queue->skb_dirtytx;
 
@@ -1798,8 +2020,8 @@ static int gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue)
         }
 
         /* If we freed a buffer, we can restart transmission, if necessary */
-        if (netif_queue_stopped(dev) && tx_queue->num_txbdfree)
-                netif_wake_queue(dev);
+        if (__netif_subqueue_stopped(dev, tx_queue->qindex) && tx_queue->num_txbdfree)
+                netif_wake_subqueue(dev, tx_queue->qindex);
 
         /* Update dirty indicators */
         tx_queue->skb_dirtytx = skb_dirtytx;
@@ -1812,19 +2034,12 @@ static int gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue)
 
 static void gfar_schedule_cleanup(struct gfar_priv_grp *gfargrp)
 {
-        struct gfar_private *priv = gfargrp->priv;
-        struct gfar_priv_tx_q *tx_queue = NULL;
-        struct gfar_priv_rx_q *rx_queue = NULL;
         unsigned long flags;
 
-        rx_queue = priv->rx_queue;
-        tx_queue = priv->tx_queue;
-        spin_lock_irqsave(&tx_queue->txlock, flags);
-        spin_lock(&rx_queue->rxlock);
-
-        if (napi_schedule_prep(&rx_queue->napi)) {
+        spin_lock_irqsave(&gfargrp->grplock, flags);
+        if (napi_schedule_prep(&gfargrp->napi)) {
                 gfar_write(&gfargrp->regs->imask, IMASK_RTX_DISABLED);
-                __napi_schedule(&rx_queue->napi);
+                __napi_schedule(&gfargrp->napi);
         } else {
                 /*
                  * Clear IEVENT, so interrupts aren't called again
@@ -1832,9 +2047,8 @@ static void gfar_schedule_cleanup(struct gfar_priv_grp *gfargrp)
                  */
                 gfar_write(&gfargrp->regs->ievent, IEVENT_RTX_MASK);
         }
+        spin_unlock_irqrestore(&gfargrp->grplock, flags);
 
-        spin_unlock(&rx_queue->rxlock);
-        spin_unlock_irqrestore(&tx_queue->txlock, flags);
 }
 
 /* Interrupt Handler for Transmit complete */
@@ -1952,6 +2166,7 @@ static int gfar_process_frame(struct net_device *dev, struct sk_buff *skb,
         fcb = (struct rxfcb *)skb->data;
 
         /* Remove the FCB from the skb */
+        skb_set_queue_mapping(skb, fcb->rq);
         /* Remove the padded bytes, if there are any */
         if (amount_pull)
                 skb_pull(skb, amount_pull);
@@ -2072,28 +2287,54 @@ int gfar_clean_rx_ring(struct gfar_priv_rx_q *rx_queue, int rx_work_limit)
 
 static int gfar_poll(struct napi_struct *napi, int budget)
 {
-        struct gfar_priv_rx_q *rx_queue = container_of(napi,
-                        struct gfar_priv_rx_q, napi);
-        struct net_device *dev = rx_queue->dev;
-        struct gfar_private *priv = netdev_priv(dev);
+        struct gfar_priv_grp *gfargrp = container_of(napi,
+                        struct gfar_priv_grp, napi);
+        struct gfar_private *priv = gfargrp->priv;
         struct gfar __iomem *regs = priv->gfargrp.regs;
         struct gfar_priv_tx_q *tx_queue = NULL;
-        int tx_cleaned = 0;
-        int rx_cleaned = 0;
+        struct gfar_priv_rx_q *rx_queue = NULL;
+        int rx_cleaned = 0, budget_per_queue = 0, rx_cleaned_per_queue = 0;
+        int tx_cleaned = 0, i, left_over_budget = budget, serviced_queues = 0;
+        int num_queues = 0;
         unsigned long flags;
 
+        num_queues = gfargrp->num_rx_queues;
+        budget_per_queue = budget/num_queues;
+
         /* Clear IEVENT, so interrupts aren't called again
          * because of the packets that have already arrived */
         gfar_write(&regs->ievent, IEVENT_RTX_MASK);
-        tx_queue = priv->tx_queue;
 
-        /* If we fail to get the lock, don't bother with the TX BDs */
-        if (spin_trylock_irqsave(&tx_queue->txlock, flags)) {
-                tx_cleaned = gfar_clean_tx_ring(tx_queue);
-                spin_unlock_irqrestore(&tx_queue->txlock, flags);
-        }
+        while (num_queues && left_over_budget) {
 
-        rx_cleaned = gfar_clean_rx_ring(rx_queue, budget);
+                budget_per_queue = left_over_budget/num_queues;
+                left_over_budget = 0;
+
+                for_each_bit(i, &gfargrp->rx_bit_map, priv->num_rx_queues) {
+                        if (test_bit(i, &serviced_queues))
+                                continue;
+                        rx_queue = priv->rx_queue[i];
+                        tx_queue = priv->tx_queue[rx_queue->qindex];
+
+                        /* If we fail to get the lock,
+                         * don't bother with the TX BDs */
+                        if (spin_trylock_irqsave(&tx_queue->txlock, flags)) {
+                                tx_cleaned += gfar_clean_tx_ring(tx_queue);
+                                spin_unlock_irqrestore(&tx_queue->txlock,
+                                                        flags);
+                        }
+
+                        rx_cleaned_per_queue = gfar_clean_rx_ring(rx_queue,
+                                                        budget_per_queue);
+                        rx_cleaned += rx_cleaned_per_queue;
+                        if(rx_cleaned_per_queue < budget_per_queue) {
+                                left_over_budget = left_over_budget +
+                                        (budget_per_queue - rx_cleaned_per_queue);
+                                set_bit(i, &serviced_queues);
+                                num_queues--;
+                        }
+                }
+        }
 
         if (tx_cleaned)
                 return budget;
@@ -2102,7 +2343,7 @@ static int gfar_poll(struct napi_struct *napi, int budget)
                 napi_complete(napi);
 
                 /* Clear the halt bit in RSTAT */
-                gfar_write(&regs->rstat, RSTAT_CLEAR_RHALT);
+                gfar_write(&regs->rstat, gfargrp->rstat);
 
                 gfar_write(&regs->imask, IMASK_DEFAULT);
 
@@ -2180,14 +2421,14 @@ static irqreturn_t gfar_interrupt(int irq, void *grp_id)
 static void adjust_link(struct net_device *dev)
 {
         struct gfar_private *priv = netdev_priv(dev);
-        struct gfar_priv_tx_q *tx_queue = NULL;
         struct gfar __iomem *regs = priv->gfargrp.regs;
         unsigned long flags;
         struct phy_device *phydev = priv->phydev;
         int new_state = 0;
 
-        tx_queue = priv->tx_queue;
-        spin_lock_irqsave(&tx_queue->txlock, flags);
+        local_irq_save(flags);
+        lock_tx_qs(priv);
+
         if (phydev->link) {
                 u32 tempval = gfar_read(&regs->maccfg2);
                 u32 ecntrl = gfar_read(&regs->ecntrl);
@@ -2252,8 +2493,8 @@ static void adjust_link(struct net_device *dev)
 
         if (new_state && netif_msg_link(priv))
                 phy_print_status(phydev);
-
-        spin_unlock_irqrestore(&tx_queue->txlock, flags);
+        unlock_tx_qs(priv);
+        local_irq_restore(flags);
 }
 
 /* Update the hash table based on the current list of multicast
@@ -2457,7 +2698,7 @@ static irqreturn_t gfar_error(int irq, void *grp_id)
                         priv->extra_stats.tx_underrun++;
 
                         /* Reactivate the Tx Queues */
-                        gfar_write(&regs->tstat, TSTAT_CLEAR_THALT);
+                        gfar_write(&regs->tstat, gfargrp->tstat);
                 }
                 if (netif_msg_tx_err(priv))
                         printk(KERN_DEBUG "%s: Transmit Error\n", dev->name);