gianfar: Add Multiple Queue Support

This patch introduces multiple Tx and Rx queues.
The incoming packets can be classified into different queues
based on filer rules (out of scope of this patch). The number
of queues enabled will be based on a DTS entries fsl,num_tx_queues
and fsl,num_rx_queues.

Although we are enabling multiple queues, the interrupt coalescing
is on per device level (etsec-1.7 doesn't support multiple rxics
and txics).

Signed-off-by: Sandeep Gopalpet <Sandeep.Kumar@freescale.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

1 files changed, 468 insertions, 227 deletions

diff --git a/drivers/net/gianfar.c b/drivers/net/gianfar.c
index fa0188e..aa258e8 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);