Subject: Re: [PATCH] net/macb: add TX multiqueue support for gem

From: Cyrille Pitchen <[email-masked]>
Date: Wed, 10 Dec 2014 16:21:02 +0100

Please ignore this second patch: it is the very same as the first one except 
for the commit message.
A .patch~ file was remaining in the directory before I called git send-email.

Sorry

Le 10/12/2014 16:03, Cyrille Pitchen a écrit :
> gem devices designed with multiqueue CANNOT work without this patch.
> 
> When probing a gem device, the driver must first prepare and enable the
> peripheral clock before accessing I/O registers. The second step is to read the
> MID register to find whether the device is a gem or an old macb IP.
> For gem devices, it reads the Design Configuration Register 6 (DCFG6) to
> compute to total number of queues, whereas macb devices always have a single
> queue.
> Only then it can call alloc_etherdev_mq() with the correct number of queues.
> This is the reason why the order of some initializations has been changed in
> macb_probe().
> Eventually, the dedicated IRQ and TX ring buffer descriptors are initialized
> for each queue.
> 
> Besides when a TX error occurs, the gem MUST be halted before writing any of
> the TBQP registers to reset the relevant queue. An immediate side effect is
> that the other queues too aren't processed anymore by the gem.
> So macb_tx_error_task() calls netif_tx_stop_all_queues() to notify the Linux
> network engine that all transmissions are stopped.
> 
> Also macb_tx_error_task() now calls spin_lock_irqsave() to prevent the
> interrupt handlers of the other queues from running as each of them may wake
> its associated queue up (please refer to macb_tx_interrupt()).
> 
> Finally, as all queues have previously been stopped, they should be restarted
> calling netif_tx_start_all_queues() and setting the TSTART bit into the Network
> Control Register. Before this patch, when dealing with a single queue, the
> driver used to defer the reset of the faulting queue and the write of the
> TSTART bit until the next call of macb_start_xmit().
> As explained before, this bit is now set by macb_tx_error_task() too. That's
> why the faulting queue MUST be reset by setting the TX_USED bit in its first
> buffer descriptor before writing the TSTART bit.
> 
> Queue 0 always exits and is the lowest priority when other queues are available.
> The higher the index of the queue is, the higher its priority is.
> 
> When transmitting frames, the TX queue is selected by the skb->queue_mapping
> value. So queue discipline can be used to define the queue priority policy.
> 
> Signed-off-by: Cyrille Pitchen <[email-masked]>
> ---
>  drivers/net/ethernet/cadence/macb.c | 442 +++++++++++++++++++++++-------------
>  drivers/net/ethernet/cadence/macb.h |  71 +++++-
>  2 files changed, 349 insertions(+), 164 deletions(-)
> 
> diff --git a/drivers/net/ethernet/cadence/macb.c b/drivers/net/ethernet/cadence/macb.c
> index 41113e5..072b76a 100644
> --- a/drivers/net/ethernet/cadence/macb.c
> +++ b/drivers/net/ethernet/cadence/macb.c
> @@ -66,23 +66,25 @@ static unsigned int macb_tx_ring_wrap(unsigned int index)
>  	return index & (TX_RING_SIZE - 1);
>  }
>  
> -static struct macb_dma_desc *macb_tx_desc(struct macb *bp, unsigned int index)
> +static struct macb_dma_desc *macb_tx_desc(struct macb_queue *queue,
> +					  unsigned int index)
>  {
> -	return &bp->tx_ring[macb_tx_ring_wrap(index)];
> +	return &queue->tx_ring[macb_tx_ring_wrap(index)];
>  }
>  
> -static struct macb_tx_skb *macb_tx_skb(struct macb *bp, unsigned int index)
> +static struct macb_tx_skb *macb_tx_skb(struct macb_queue *queue,
> +				       unsigned int index)
>  {
> -	return &bp->tx_skb[macb_tx_ring_wrap(index)];
> +	return &queue->tx_skb[macb_tx_ring_wrap(index)];
>  }
>  
> -static dma_addr_t macb_tx_dma(struct macb *bp, unsigned int index)
> +static dma_addr_t macb_tx_dma(struct macb_queue *queue, unsigned int index)
>  {
>  	dma_addr_t offset;
>  
>  	offset = macb_tx_ring_wrap(index) * sizeof(struct macb_dma_desc);
>  
> -	return bp->tx_ring_dma + offset;
> +	return queue->tx_ring_dma + offset;
>  }
>  
>  static unsigned int macb_rx_ring_wrap(unsigned int index)
> @@ -490,38 +492,42 @@ static void macb_tx_unmap(struct macb *bp, struct macb_tx_skb *tx_skb)
>  
>  static void macb_tx_error_task(struct work_struct *work)
>  {
> -	struct macb	*bp = container_of(work, struct macb, tx_error_task);
> +	struct macb_queue	*queue = container_of(work, struct macb_queue,
> +						      tx_error_task);
> +	struct macb		*bp = queue->bp;
>  	struct macb_tx_skb	*tx_skb;
> +	struct macb_dma_desc	*desc;
>  	struct sk_buff		*skb;
>  	unsigned int		tail;
> +	unsigned long		flags;
> +
> +	netdev_vdbg(bp->dev, "macb_tx_error_task: q = %u, t = %u, h = %u\n",
> +		    queue - bp->queues, queue->tx_tail, queue->tx_head);
>  
> -	netdev_vdbg(bp->dev, "macb_tx_error_task: t = %u, h = %u\n",
> -		    bp->tx_tail, bp->tx_head);
> +	spin_lock_irqsave(&bp->lock, flags);
>  
>  	/* Make sure nobody is trying to queue up new packets */
> -	netif_stop_queue(bp->dev);
> +	netif_tx_stop_all_queues(bp->dev);
>  
>  	/*
>  	 * Stop transmission now
>  	 * (in case we have just queued new packets)
> +	 * macb/gem must be halted to write TBQP register
>  	 */
>  	if (macb_halt_tx(bp))
>  		/* Just complain for now, reinitializing TX path can be good */
>  		netdev_err(bp->dev, "BUG: halt tx timed out\n");
>  
> -	/* No need for the lock here as nobody will interrupt us anymore */
> -
>  	/*
>  	 * Treat frames in TX queue including the ones that caused the error.
>  	 * Free transmit buffers in upper layer.
>  	 */
> -	for (tail = bp->tx_tail; tail != bp->tx_head; tail++) {
> -		struct macb_dma_desc	*desc;
> -		u32			ctrl;
> +	for (tail = queue->tx_tail; tail != queue->tx_head; tail++) {
> +		u32	ctrl;
>  
> -		desc = macb_tx_desc(bp, tail);
> +		desc = macb_tx_desc(queue, tail);
>  		ctrl = desc->ctrl;
> -		tx_skb = macb_tx_skb(bp, tail);
> +		tx_skb = macb_tx_skb(queue, tail);
>  		skb = tx_skb->skb;
>  
>  		if (ctrl & MACB_BIT(TX_USED)) {
> @@ -529,7 +535,7 @@ static void macb_tx_error_task(struct work_struct *work)
>  			while (!skb) {
>  				macb_tx_unmap(bp, tx_skb);
>  				tail++;
> -				tx_skb = macb_tx_skb(bp, tail);
> +				tx_skb = macb_tx_skb(queue, tail);
>  				skb = tx_skb->skb;
>  			}
>  
> @@ -558,45 +564,56 @@ static void macb_tx_error_task(struct work_struct *work)
>  		macb_tx_unmap(bp, tx_skb);
>  	}
>  
> +	/* Set end of TX queue */
> +	desc = macb_tx_desc(queue, 0);
> +	desc->addr = 0;
> +	desc->ctrl = MACB_BIT(TX_USED);
> +
>  	/* Make descriptor updates visible to hardware */
>  	wmb();
>  
>  	/* Reinitialize the TX desc queue */
> -	macb_writel(bp, TBQP, bp->tx_ring_dma);
> +	queue_writel(queue, TBQP, queue->tx_ring_dma);
>  	/* Make TX ring reflect state of hardware */
> -	bp->tx_head = bp->tx_tail = 0;
> -
> -	/* Now we are ready to start transmission again */
> -	netif_wake_queue(bp->dev);
> +	queue->tx_head = 0;
> +	queue->tx_tail = 0;
>  
>  	/* Housework before enabling TX IRQ */
>  	macb_writel(bp, TSR, macb_readl(bp, TSR));
> -	macb_writel(bp, IER, MACB_TX_INT_FLAGS);
> +	queue_writel(queue, IER, MACB_TX_INT_FLAGS);
> +
> +	/* Now we are ready to start transmission again */
> +	netif_tx_start_all_queues(bp->dev);
> +	macb_writel(bp, NCR, macb_readl(bp, NCR) | MACB_BIT(TSTART));
> +
> +	spin_unlock_irqrestore(&bp->lock, flags);
>  }
>  
> -static void macb_tx_interrupt(struct macb *bp)
> +static void macb_tx_interrupt(struct macb_queue *queue)
>  {
>  	unsigned int tail;
>  	unsigned int head;
>  	u32 status;
> +	struct macb *bp = queue->bp;
> +	u16 queue_index = queue - bp->queues;
>  
>  	status = macb_readl(bp, TSR);
>  	macb_writel(bp, TSR, status);
>  
>  	if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
> -		macb_writel(bp, ISR, MACB_BIT(TCOMP));
> +		queue_writel(queue, ISR, MACB_BIT(TCOMP));
>  
>  	netdev_vdbg(bp->dev, "macb_tx_interrupt status = 0x%03lx\n",
>  		(unsigned long)status);
>  
> -	head = bp->tx_head;
> -	for (tail = bp->tx_tail; tail != head; tail++) {
> +	head = queue->tx_head;
> +	for (tail = queue->tx_tail; tail != head; tail++) {
>  		struct macb_tx_skb	*tx_skb;
>  		struct sk_buff		*skb;
>  		struct macb_dma_desc	*desc;
>  		u32			ctrl;
>  
> -		desc = macb_tx_desc(bp, tail);
> +		desc = macb_tx_desc(queue, tail);
>  
>  		/* Make hw descriptor updates visible to CPU */
>  		rmb();
> @@ -611,7 +628,7 @@ static void macb_tx_interrupt(struct macb *bp)
>  
>  		/* Process all buffers of the current transmitted frame */
>  		for (;; tail++) {
> -			tx_skb = macb_tx_skb(bp, tail);
> +			tx_skb = macb_tx_skb(queue, tail);
>  			skb = tx_skb->skb;
>  
>  			/* First, update TX stats if needed */
> @@ -634,11 +651,11 @@ static void macb_tx_interrupt(struct macb *bp)
>  		}
>  	}
>  
> -	bp->tx_tail = tail;
> -	if (netif_queue_stopped(bp->dev)
> -			&& CIRC_CNT(bp->tx_head, bp->tx_tail,
> -				    TX_RING_SIZE) <= MACB_TX_WAKEUP_THRESH)
> -		netif_wake_queue(bp->dev);
> +	queue->tx_tail = tail;
> +	if (__netif_subqueue_stopped(bp->dev, queue_index) &&
> +	    CIRC_CNT(queue->tx_head, queue->tx_tail,
> +		     TX_RING_SIZE) <= MACB_TX_WAKEUP_THRESH)
> +		netif_wake_subqueue(bp->dev, queue_index);
>  }
>  
>  static void gem_rx_refill(struct macb *bp)
> @@ -949,11 +966,12 @@ static int macb_poll(struct napi_struct *napi, int budget)
>  
>  static irqreturn_t macb_interrupt(int irq, void *dev_id)
>  {
> -	struct net_device *dev = dev_id;
> -	struct macb *bp = netdev_priv(dev);
> +	struct macb_queue *queue = dev_id;
> +	struct macb *bp = queue->bp;
> +	struct net_device *dev = bp->dev;
>  	u32 status;
>  
> -	status = macb_readl(bp, ISR);
> +	status = queue_readl(queue, ISR);
>  
>  	if (unlikely(!status))
>  		return IRQ_NONE;
> @@ -963,11 +981,12 @@ static irqreturn_t macb_interrupt(int irq, void *dev_id)
>  	while (status) {
>  		/* close possible race with dev_close */
>  		if (unlikely(!netif_running(dev))) {
> -			macb_writel(bp, IDR, -1);
> +			queue_writel(queue, IDR, -1);
>  			break;
>  		}
>  
> -		netdev_vdbg(bp->dev, "isr = 0x%08lx\n", (unsigned long)status);
> +		netdev_vdbg(bp->dev, "queue = %u, isr = 0x%08lx\n",
> +			    queue - bp->queues, (unsigned long)status);
>  
>  		if (status & MACB_RX_INT_FLAGS) {
>  			/*
> @@ -977,9 +996,9 @@ static irqreturn_t macb_interrupt(int irq, void *dev_id)
>  			 * is already scheduled, so disable interrupts
>  			 * now.
>  			 */
> -			macb_writel(bp, IDR, MACB_RX_INT_FLAGS);
> +			queue_writel(queue, IDR, MACB_RX_INT_FLAGS);
>  			if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
> -				macb_writel(bp, ISR, MACB_BIT(RCOMP));
> +				queue_writel(queue, ISR, MACB_BIT(RCOMP));
>  
>  			if (napi_schedule_prep(&bp->napi)) {
>  				netdev_vdbg(bp->dev, "scheduling RX softirq\n");
> @@ -988,17 +1007,17 @@ static irqreturn_t macb_interrupt(int irq, void *dev_id)
>  		}
>  
>  		if (unlikely(status & (MACB_TX_ERR_FLAGS))) {
> -			macb_writel(bp, IDR, MACB_TX_INT_FLAGS);
> -			schedule_work(&bp->tx_error_task);
> +			queue_writel(queue, IDR, MACB_TX_INT_FLAGS);
> +			schedule_work(&queue->tx_error_task);
>  
>  			if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
> -				macb_writel(bp, ISR, MACB_TX_ERR_FLAGS);
> +				queue_writel(queue, ISR, MACB_TX_ERR_FLAGS);
>  
>  			break;
>  		}
>  
>  		if (status & MACB_BIT(TCOMP))
> -			macb_tx_interrupt(bp);
> +			macb_tx_interrupt(queue);
>  
>  		/*
>  		 * Link change detection isn't possible with RMII, so we'll
> @@ -1013,7 +1032,7 @@ static irqreturn_t macb_interrupt(int irq, void *dev_id)
>  				bp->hw_stats.macb.rx_overruns++;
>  
>  			if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
> -				macb_writel(bp, ISR, MACB_BIT(ISR_ROVR));
> +				queue_writel(queue, ISR, MACB_BIT(ISR_ROVR));
>  		}
>  
>  		if (status & MACB_BIT(HRESP)) {
> @@ -1025,10 +1044,10 @@ static irqreturn_t macb_interrupt(int irq, void *dev_id)
>  			netdev_err(dev, "DMA bus error: HRESP not OK\n");
>  
>  			if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
> -				macb_writel(bp, ISR, MACB_BIT(HRESP));
> +				queue_writel(queue, ISR, MACB_BIT(HRESP));
>  		}
>  
> -		status = macb_readl(bp, ISR);
> +		status = queue_readl(queue, ISR);
>  	}
>  
>  	spin_unlock(&bp->lock);
> @@ -1043,10 +1062,14 @@ static irqreturn_t macb_interrupt(int irq, void *dev_id)
>   */
>  static void macb_poll_controller(struct net_device *dev)
>  {
> +	struct macb *bp = netdev_priv(dev);
> +	struct macb_queue *queue;
>  	unsigned long flags;
> +	unsigned int q;
>  
>  	local_irq_save(flags);
> -	macb_interrupt(dev->irq, dev);
> +	for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue)
> +		macb_interrupt(dev->irq, queue);
>  	local_irq_restore(flags);
>  }
>  #endif
> @@ -1058,10 +1081,11 @@ static inline unsigned int macb_count_tx_descriptors(struct macb *bp,
>  }
>  
>  static unsigned int macb_tx_map(struct macb *bp,
> +				struct macb_queue *queue,
>  				struct sk_buff *skb)
>  {
>  	dma_addr_t mapping;
> -	unsigned int len, entry, i, tx_head = bp->tx_head;
> +	unsigned int len, entry, i, tx_head = queue->tx_head;
>  	struct macb_tx_skb *tx_skb = NULL;
>  	struct macb_dma_desc *desc;
>  	unsigned int offset, size, count = 0;
> @@ -1075,7 +1099,7 @@ static unsigned int macb_tx_map(struct macb *bp,
>  	while (len) {
>  		size = min(len, bp->max_tx_length);
>  		entry = macb_tx_ring_wrap(tx_head);
> -		tx_skb = &bp->tx_skb[entry];
> +		tx_skb = &queue->tx_skb[entry];
>  
>  		mapping = dma_map_single(&bp->pdev->dev,
>  					 skb->data + offset,
> @@ -1104,7 +1128,7 @@ static unsigned int macb_tx_map(struct macb *bp,
>  		while (len) {
>  			size = min(len, bp->max_tx_length);
>  			entry = macb_tx_ring_wrap(tx_head);
> -			tx_skb = &bp->tx_skb[entry];
> +			tx_skb = &queue->tx_skb[entry];
>  
>  			mapping = skb_frag_dma_map(&bp->pdev->dev, frag,
>  						   offset, size, DMA_TO_DEVICE);
> @@ -1143,14 +1167,14 @@ static unsigned int macb_tx_map(struct macb *bp,
>  	i = tx_head;
>  	entry = macb_tx_ring_wrap(i);
>  	ctrl = MACB_BIT(TX_USED);
> -	desc = &bp->tx_ring[entry];
> +	desc = &queue->tx_ring[entry];
>  	desc->ctrl = ctrl;
>  
>  	do {
>  		i--;
>  		entry = macb_tx_ring_wrap(i);
> -		tx_skb = &bp->tx_skb[entry];
> -		desc = &bp->tx_ring[entry];
> +		tx_skb = &queue->tx_skb[entry];
> +		desc = &queue->tx_ring[entry];
>  
>  		ctrl = (u32)tx_skb->size;
>  		if (eof) {
> @@ -1167,17 +1191,17 @@ static unsigned int macb_tx_map(struct macb *bp,
>  		 */
>  		wmb();
>  		desc->ctrl = ctrl;
> -	} while (i != bp->tx_head);
> +	} while (i != queue->tx_head);
>  
> -	bp->tx_head = tx_head;
> +	queue->tx_head = tx_head;
>  
>  	return count;
>  
>  dma_error:
>  	netdev_err(bp->dev, "TX DMA map failed\n");
>  
> -	for (i = bp->tx_head; i != tx_head; i++) {
> -		tx_skb = macb_tx_skb(bp, i);
> +	for (i = queue->tx_head; i != tx_head; i++) {
> +		tx_skb = macb_tx_skb(queue, i);
>  
>  		macb_tx_unmap(bp, tx_skb);
>  	}
> @@ -1187,14 +1211,16 @@ dma_error:
>  
>  static int macb_start_xmit(struct sk_buff *skb, struct net_device *dev)
>  {
> +	u16 queue_index = skb_get_queue_mapping(skb);
>  	struct macb *bp = netdev_priv(dev);
> +	struct macb_queue *queue = &bp->queues[queue_index];
>  	unsigned long flags;
>  	unsigned int count, nr_frags, frag_size, f;
>  
>  #if defined(DEBUG) && defined(VERBOSE_DEBUG)
>  	netdev_vdbg(bp->dev,
> -		   "start_xmit: len %u head %p data %p tail %p end %p\n",
> -		   skb->len, skb->head, skb->data,
> +		   "start_xmit: queue %hu len %u head %p data %p tail %p end %p\n",
> +		   queue_index, skb->len, skb->head, skb->data,
>  		   skb_tail_pointer(skb), skb_end_pointer(skb));
>  	print_hex_dump(KERN_DEBUG, "data: ", DUMP_PREFIX_OFFSET, 16, 1,
>  		       skb->data, 16, true);
> @@ -1214,16 +1240,16 @@ static int macb_start_xmit(struct sk_buff *skb, struct net_device *dev)
>  	spin_lock_irqsave(&bp->lock, flags);
>  
>  	/* This is a hard error, log it. */
> -	if (CIRC_SPACE(bp->tx_head, bp->tx_tail, TX_RING_SIZE) < count) {
> -		netif_stop_queue(dev);
> +	if (CIRC_SPACE(queue->tx_head, queue->tx_tail, TX_RING_SIZE) < count) {
> +		netif_stop_subqueue(dev, queue_index);
>  		spin_unlock_irqrestore(&bp->lock, flags);
>  		netdev_dbg(bp->dev, "tx_head = %u, tx_tail = %u\n",
> -			   bp->tx_head, bp->tx_tail);
> +			   queue->tx_head, queue->tx_tail);
>  		return NETDEV_TX_BUSY;
>  	}
>  
>  	/* Map socket buffer for DMA transfer */
> -	if (!macb_tx_map(bp, skb)) {
> +	if (!macb_tx_map(bp, queue, skb)) {
>  		dev_kfree_skb_any(skb);
>  		goto unlock;
>  	}
> @@ -1235,8 +1261,8 @@ static int macb_start_xmit(struct sk_buff *skb, struct net_device *dev)
>  
>  	macb_writel(bp, NCR, macb_readl(bp, NCR) | MACB_BIT(TSTART));
>  
> -	if (CIRC_SPACE(bp->tx_head, bp->tx_tail, TX_RING_SIZE) < 1)
> -		netif_stop_queue(dev);
> +	if (CIRC_SPACE(queue->tx_head, queue->tx_tail, TX_RING_SIZE) < 1)
> +		netif_stop_subqueue(dev, queue_index);
>  
>  unlock:
>  	spin_unlock_irqrestore(&bp->lock, flags);
> @@ -1304,20 +1330,24 @@ static void macb_free_rx_buffers(struct macb *bp)
>  
>  static void macb_free_consistent(struct macb *bp)
>  {
> -	if (bp->tx_skb) {
> -		kfree(bp->tx_skb);
> -		bp->tx_skb = NULL;
> -	}
> +	struct macb_queue *queue;
> +	unsigned int q;
> +
>  	bp->macbgem_ops.mog_free_rx_buffers(bp);
>  	if (bp->rx_ring) {
>  		dma_free_coherent(&bp->pdev->dev, RX_RING_BYTES,
>  				  bp->rx_ring, bp->rx_ring_dma);
>  		bp->rx_ring = NULL;
>  	}
> -	if (bp->tx_ring) {
> -		dma_free_coherent(&bp->pdev->dev, TX_RING_BYTES,
> -				  bp->tx_ring, bp->tx_ring_dma);
> -		bp->tx_ring = NULL;
> +
> +	for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
> +		kfree(queue->tx_skb);
> +		queue->tx_skb = NULL;
> +		if (queue->tx_ring) {
> +			dma_free_coherent(&bp->pdev->dev, TX_RING_BYTES,
> +					  queue->tx_ring, queue->tx_ring_dma);
> +			queue->tx_ring = NULL;
> +		}
>  	}
>  }
>  
> @@ -1354,12 +1384,27 @@ static int macb_alloc_rx_buffers(struct macb *bp)
>  
>  static int macb_alloc_consistent(struct macb *bp)
>  {
> +	struct macb_queue *queue;
> +	unsigned int q;
>  	int size;
>  
> -	size = TX_RING_SIZE * sizeof(struct macb_tx_skb);
> -	bp->tx_skb = kmalloc(size, GFP_KERNEL);
> -	if (!bp->tx_skb)
> -		goto out_err;
> +	for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
> +		size = TX_RING_BYTES;
> +		queue->tx_ring = dma_alloc_coherent(&bp->pdev->dev, size,
> +						    &queue->tx_ring_dma,
> +						    GFP_KERNEL);
> +		if (!queue->tx_ring)
> +			goto out_err;
> +		netdev_dbg(bp->dev,
> +			   "Allocated TX ring for queue %u of %d bytes at %08lx (mapped %p)\n",
> +			   q, size, (unsigned long)queue->tx_ring_dma,
> +			   queue->tx_ring);
> +
> +		size = TX_RING_SIZE * sizeof(struct macb_tx_skb);
> +		queue->tx_skb = kmalloc(size, GFP_KERNEL);
> +		if (!queue->tx_skb)
> +			goto out_err;
> +	}
>  
>  	size = RX_RING_BYTES;
>  	bp->rx_ring = dma_alloc_coherent(&bp->pdev->dev, size,
> @@ -1370,15 +1415,6 @@ static int macb_alloc_consistent(struct macb *bp)
>  		   "Allocated RX ring of %d bytes at %08lx (mapped %p)\n",
>  		   size, (unsigned long)bp->rx_ring_dma, bp->rx_ring);
>  
> -	size = TX_RING_BYTES;
> -	bp->tx_ring = dma_alloc_coherent(&bp->pdev->dev, size,
> -					 &bp->tx_ring_dma, GFP_KERNEL);
> -	if (!bp->tx_ring)
> -		goto out_err;
> -	netdev_dbg(bp->dev,
> -		   "Allocated TX ring of %d bytes at %08lx (mapped %p)\n",
> -		   size, (unsigned long)bp->tx_ring_dma, bp->tx_ring);
> -
>  	if (bp->macbgem_ops.mog_alloc_rx_buffers(bp))
>  		goto out_err;
>  
> @@ -1391,15 +1427,22 @@ out_err:
>  
>  static void gem_init_rings(struct macb *bp)
>  {
> +	struct macb_queue *queue;
> +	unsigned int q;
>  	int i;
>  
> -	for (i = 0; i < TX_RING_SIZE; i++) {
> -		bp->tx_ring[i].addr = 0;
> -		bp->tx_ring[i].ctrl = MACB_BIT(TX_USED);
> +	for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
> +		for (i = 0; i < TX_RING_SIZE; i++) {
> +			queue->tx_ring[i].addr = 0;
> +			queue->tx_ring[i].ctrl = MACB_BIT(TX_USED);
> +		}
> +		queue->tx_ring[TX_RING_SIZE - 1].ctrl |= MACB_BIT(TX_WRAP);
> +		queue->tx_head = 0;
> +		queue->tx_tail = 0;
>  	}
> -	bp->tx_ring[TX_RING_SIZE - 1].ctrl |= MACB_BIT(TX_WRAP);
>  
> -	bp->rx_tail = bp->rx_prepared_head = bp->tx_head = bp->tx_tail = 0;
> +	bp->rx_tail = 0;
> +	bp->rx_prepared_head = 0;
>  
>  	gem_rx_refill(bp);
>  }
> @@ -1418,16 +1461,21 @@ static void macb_init_rings(struct macb *bp)
>  	bp->rx_ring[RX_RING_SIZE - 1].addr |= MACB_BIT(RX_WRAP);
>  
>  	for (i = 0; i < TX_RING_SIZE; i++) {
> -		bp->tx_ring[i].addr = 0;
> -		bp->tx_ring[i].ctrl = MACB_BIT(TX_USED);
> +		bp->queues[0].tx_ring[i].addr = 0;
> +		bp->queues[0].tx_ring[i].ctrl = MACB_BIT(TX_USED);
> +		bp->queues[0].tx_head = 0;
> +		bp->queues[0].tx_tail = 0;
>  	}
> -	bp->tx_ring[TX_RING_SIZE - 1].ctrl |= MACB_BIT(TX_WRAP);
> +	bp->queues[0].tx_ring[TX_RING_SIZE - 1].ctrl |= MACB_BIT(TX_WRAP);
>  
> -	bp->rx_tail = bp->tx_head = bp->tx_tail = 0;
> +	bp->rx_tail = 0;
>  }
>  
>  static void macb_reset_hw(struct macb *bp)
>  {
> +	struct macb_queue *queue;
> +	unsigned int q;
> +
>  	/*
>  	 * Disable RX and TX (XXX: Should we halt the transmission
>  	 * more gracefully?)
> @@ -1442,8 +1490,10 @@ static void macb_reset_hw(struct macb *bp)
>  	macb_writel(bp, RSR, -1);
>  
>  	/* Disable all interrupts */
> -	macb_writel(bp, IDR, -1);
> -	macb_readl(bp, ISR);
> +	for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
> +		queue_writel(queue, IDR, -1);
> +		queue_readl(queue, ISR);
> +	}
>  }
>  
>  static u32 gem_mdc_clk_div(struct macb *bp)
> @@ -1540,6 +1590,9 @@ static void macb_configure_dma(struct macb *bp)
>  
>  static void macb_init_hw(struct macb *bp)
>  {
> +	struct macb_queue *queue;
> +	unsigned int q;
> +
>  	u32 config;
>  
>  	macb_reset_hw(bp);
> @@ -1565,16 +1618,18 @@ static void macb_init_hw(struct macb *bp)
>  
>  	/* Initialize TX and RX buffers */
>  	macb_writel(bp, RBQP, bp->rx_ring_dma);
> -	macb_writel(bp, TBQP, bp->tx_ring_dma);
> +	for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
> +		queue_writel(queue, TBQP, queue->tx_ring_dma);
> +
> +		/* Enable interrupts */
> +		queue_writel(queue, IER,
> +			     MACB_RX_INT_FLAGS |
> +			     MACB_TX_INT_FLAGS |
> +			     MACB_BIT(HRESP));
> +	}
>  
>  	/* Enable TX and RX */
>  	macb_writel(bp, NCR, MACB_BIT(RE) | MACB_BIT(TE) | MACB_BIT(MPE));
> -
> -	/* Enable interrupts */
> -	macb_writel(bp, IER, (MACB_RX_INT_FLAGS
> -			      | MACB_TX_INT_FLAGS
> -			      | MACB_BIT(HRESP)));
> -
>  }
>  
>  /*
> @@ -1736,7 +1791,7 @@ static int macb_open(struct net_device *dev)
>  	/* schedule a link state check */
>  	phy_start(bp->phy_dev);
>  
> -	netif_start_queue(dev);
> +	netif_tx_start_all_queues(dev);
>  
>  	return 0;
>  }
> @@ -1746,7 +1801,7 @@ static int macb_close(struct net_device *dev)
>  	struct macb *bp = netdev_priv(dev);
>  	unsigned long flags;
>  
> -	netif_stop_queue(dev);
> +	netif_tx_stop_all_queues(dev);
>  	napi_disable(&bp->napi);
>  
>  	if (bp->phy_dev)
> @@ -1895,8 +1950,8 @@ static void macb_get_regs(struct net_device *dev, struct ethtool_regs *regs,
>  	regs->version = (macb_readl(bp, MID) & ((1 << MACB_REV_SIZE) - 1))
>  			| MACB_GREGS_VERSION;
>  
> -	tail = macb_tx_ring_wrap(bp->tx_tail);
> -	head = macb_tx_ring_wrap(bp->tx_head);
> +	tail = macb_tx_ring_wrap(bp->queues[0].tx_tail);
> +	head = macb_tx_ring_wrap(bp->queues[0].tx_head);
>  
>  	regs_buff[0]  = macb_readl(bp, NCR);
>  	regs_buff[1]  = macb_or_gem_readl(bp, NCFGR);
> @@ -1909,8 +1964,8 @@ static void macb_get_regs(struct net_device *dev, struct ethtool_regs *regs,
>  
>  	regs_buff[8]  = tail;
>  	regs_buff[9]  = head;
> -	regs_buff[10] = macb_tx_dma(bp, tail);
> -	regs_buff[11] = macb_tx_dma(bp, head);
> +	regs_buff[10] = macb_tx_dma(&bp->queues[0], tail);
> +	regs_buff[11] = macb_tx_dma(&bp->queues[0], head);
>  
>  	if (macb_is_gem(bp)) {
>  		regs_buff[12] = gem_readl(bp, USRIO);
> @@ -2061,16 +2116,44 @@ static void macb_configure_caps(struct macb *bp)
>  	netdev_dbg(bp->dev, "Cadence caps 0x%08x\n", bp->caps);
>  }
>  
> +static void macb_probe_queues(void __iomem *mem,
> +			      unsigned int *queue_mask,
> +			      unsigned int *num_queues)
> +{
> +	unsigned int q;
> +	u32 mid;
> +
> +	*queue_mask = 0x1;
> +	*num_queues = 1;
> +
> +	/* is it macb or gem ? */
> +	mid = __raw_readl(mem + MACB_MID);
> +	if (MACB_BFEXT(IDNUM, mid) != 0x2)
> +		return;
> +
> +	/* bit 0 is never set but queue 0 always exists */
> +	*queue_mask = __raw_readl(mem + GEM_DCFG6) & 0xff;
> +	*queue_mask |= 0x1;
> +
> +	for (q = 1; q < MACB_MAX_QUEUES; ++q)
> +		if (*queue_mask & (1 << q))
> +			(*num_queues)++;
> +}
> +
>  static int __init macb_probe(struct platform_device *pdev)
>  {
>  	struct macb_platform_data *pdata;
>  	struct resource *regs;
>  	struct net_device *dev;
>  	struct macb *bp;
> +	struct macb_queue *queue;
>  	struct phy_device *phydev;
>  	u32 config;
>  	int err = -ENXIO;
>  	const char *mac;
> +	void __iomem *mem;
> +	unsigned int q, queue_mask, num_queues, q_irq = 0;
> +	struct clk *pclk, *hclk, *tx_clk;
>  
>  	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
>  	if (!regs) {
> @@ -2078,72 +2161,106 @@ static int __init macb_probe(struct platform_device *pdev)
>  		goto err_out;
>  	}
>  
> -	err = -ENOMEM;
> -	dev = alloc_etherdev(sizeof(*bp));
> -	if (!dev)
> -		goto err_out;
> -
> -	SET_NETDEV_DEV(dev, &pdev->dev);
> -
> -	bp = netdev_priv(dev);
> -	bp->pdev = pdev;
> -	bp->dev = dev;
> -
> -	spin_lock_init(&bp->lock);
> -	INIT_WORK(&bp->tx_error_task, macb_tx_error_task);
> -
> -	bp->pclk = devm_clk_get(&pdev->dev, "pclk");
> -	if (IS_ERR(bp->pclk)) {
> -		err = PTR_ERR(bp->pclk);
> +	pclk = devm_clk_get(&pdev->dev, "pclk");
> +	if (IS_ERR(pclk)) {
> +		err = PTR_ERR(pclk);
>  		dev_err(&pdev->dev, "failed to get macb_clk (%u)\n", err);
> -		goto err_out_free_dev;
> +		goto err_out;
>  	}
>  
> -	bp->hclk = devm_clk_get(&pdev->dev, "hclk");
> -	if (IS_ERR(bp->hclk)) {
> -		err = PTR_ERR(bp->hclk);
> +	hclk = devm_clk_get(&pdev->dev, "hclk");
> +	if (IS_ERR(hclk)) {
> +		err = PTR_ERR(hclk);
>  		dev_err(&pdev->dev, "failed to get hclk (%u)\n", err);
> -		goto err_out_free_dev;
> +		goto err_out;
>  	}
>  
> -	bp->tx_clk = devm_clk_get(&pdev->dev, "tx_clk");
> +	tx_clk = devm_clk_get(&pdev->dev, "tx_clk");
>  
> -	err = clk_prepare_enable(bp->pclk);
> +	err = clk_prepare_enable(pclk);
>  	if (err) {
>  		dev_err(&pdev->dev, "failed to enable pclk (%u)\n", err);
> -		goto err_out_free_dev;
> +		goto err_out;
>  	}
>  
> -	err = clk_prepare_enable(bp->hclk);
> +	err = clk_prepare_enable(hclk);
>  	if (err) {
>  		dev_err(&pdev->dev, "failed to enable hclk (%u)\n", err);
>  		goto err_out_disable_pclk;
>  	}
>  
> -	if (!IS_ERR(bp->tx_clk)) {
> -		err = clk_prepare_enable(bp->tx_clk);
> +	if (!IS_ERR(tx_clk)) {
> +		err = clk_prepare_enable(tx_clk);
>  		if (err) {
>  			dev_err(&pdev->dev, "failed to enable tx_clk (%u)\n",
> -					err);
> +				err);
>  			goto err_out_disable_hclk;
>  		}
>  	}
>  
> -	bp->regs = devm_ioremap(&pdev->dev, regs->start, resource_size(regs));
> -	if (!bp->regs) {
> +	err = -ENOMEM;
> +	mem = devm_ioremap(&pdev->dev, regs->start, resource_size(regs));
> +	if (!mem) {
>  		dev_err(&pdev->dev, "failed to map registers, aborting.\n");
> -		err = -ENOMEM;
>  		goto err_out_disable_clocks;
>  	}
>  
> -	dev->irq = platform_get_irq(pdev, 0);
> -	err = devm_request_irq(&pdev->dev, dev->irq, macb_interrupt, 0,
> -			dev->name, dev);
> -	if (err) {
> -		dev_err(&pdev->dev, "Unable to request IRQ %d (error %d)\n",
> -			dev->irq, err);
> +	macb_probe_queues(mem, &queue_mask, &num_queues);
> +	dev = alloc_etherdev_mq(sizeof(*bp), num_queues);
> +	if (!dev)
>  		goto err_out_disable_clocks;
> +
> +	SET_NETDEV_DEV(dev, &pdev->dev);
> +
> +	bp = netdev_priv(dev);
> +	bp->pdev = pdev;
> +	bp->dev = dev;
> +	bp->regs = mem;
> +	bp->num_queues = num_queues;
> +	bp->pclk = pclk;
> +	bp->hclk = hclk;
> +	bp->tx_clk = tx_clk;
> +
> +	bp->queues[0].bp = bp;
> +	bp->queues[0].ISR  = MACB_ISR;
> +	bp->queues[0].IER  = MACB_IER;
> +	bp->queues[0].IDR  = MACB_IDR;
> +	bp->queues[0].IMR  = MACB_IMR;
> +	bp->queues[0].TBQP = MACB_TBQP;
> +	for (q = 1, queue = &bp->queues[1]; q < MACB_MAX_QUEUES; ++q) {
> +		if (!(queue_mask & (1 << q)))
> +			continue;
> +
> +		queue->bp = bp;
> +		queue->ISR  = (q-1) * sizeof(u32) + GEM_ISR1;
> +		queue->IER  = (q-1) * sizeof(u32) + GEM_IER1;
> +		queue->IDR  = (q-1) * sizeof(u32) + GEM_IDR1;
> +		queue->IMR  = (q-1) * sizeof(u32) + GEM_IMR1;
> +		queue->TBQP = (q-1) * sizeof(u32) + GEM_TBQP1;
> +		queue++;
> +	}
> +
> +	spin_lock_init(&bp->lock);
> +
> +	for (q = 0, queue = bp->queues; q < MACB_MAX_QUEUES; ++q) {
> +		if (!(queue_mask & (1 << q)))
> +			continue;
> +
> +		queue->irq = platform_get_irq(pdev, q);
> +		err = devm_request_irq(&pdev->dev, queue->irq, macb_interrupt,
> +				       0, dev->name, queue);
> +		if (err) {
> +			dev_err(&pdev->dev,
> +				"Unable to request IRQ %d (error %d)\n",
> +				queue->irq, err);
> +			goto err_out_free_irq;
> +		}
> +
> +		INIT_WORK(&queue->tx_error_task, macb_tx_error_task);
> +		queue++;
> +		q_irq++;
>  	}
> +	dev->irq = bp->queues[0].irq;
>  
>  	dev->netdev_ops = &macb_netdev_ops;
>  	netif_napi_add(dev, &bp->napi, macb_poll, 64);
> @@ -2219,7 +2336,7 @@ static int __init macb_probe(struct platform_device *pdev)
>  	err = register_netdev(dev);
>  	if (err) {
>  		dev_err(&pdev->dev, "Cannot register net device, aborting.\n");
> -		goto err_out_disable_clocks;
> +		goto err_out_free_irq;
>  	}
>  
>  	err = macb_mii_init(bp);
> @@ -2242,15 +2359,17 @@ static int __init macb_probe(struct platform_device *pdev)
>  
>  err_out_unregister_netdev:
>  	unregister_netdev(dev);
> +err_out_free_irq:
> +	for (q = 0, queue = bp->queues; q < q_irq; ++q, ++queue)
> +		devm_free_irq(&pdev->dev, queue->irq, queue);
> +	free_netdev(dev);
>  err_out_disable_clocks:
> -	if (!IS_ERR(bp->tx_clk))
> -		clk_disable_unprepare(bp->tx_clk);
> +	if (!IS_ERR(tx_clk))
> +		clk_disable_unprepare(tx_clk);
>  err_out_disable_hclk:
> -	clk_disable_unprepare(bp->hclk);
> +	clk_disable_unprepare(hclk);
>  err_out_disable_pclk:
> -	clk_disable_unprepare(bp->pclk);
> -err_out_free_dev:
> -	free_netdev(dev);
> +	clk_disable_unprepare(pclk);
>  err_out:
>  	return err;
>  }
> @@ -2259,6 +2378,8 @@ static int __exit macb_remove(struct platform_device *pdev)
>  {
>  	struct net_device *dev;
>  	struct macb *bp;
> +	struct macb_queue *queue;
> +	unsigned int q;
>  
>  	dev = platform_get_drvdata(pdev);
>  
> @@ -2270,11 +2391,14 @@ static int __exit macb_remove(struct platform_device *pdev)
>  		kfree(bp->mii_bus->irq);
>  		mdiobus_free(bp->mii_bus);
>  		unregister_netdev(dev);
> +		queue = bp->queues;
> +		for (q = 0; q < bp->num_queues; ++q, ++queue)
> +			devm_free_irq(&pdev->dev, queue->irq, queue);
> +		free_netdev(dev);
>  		if (!IS_ERR(bp->tx_clk))
>  			clk_disable_unprepare(bp->tx_clk);
>  		clk_disable_unprepare(bp->hclk);
>  		clk_disable_unprepare(bp->pclk);
> -		free_netdev(dev);
>  	}
>  
>  	return 0;
> diff --git a/drivers/net/ethernet/cadence/macb.h b/drivers/net/ethernet/cadence/macb.h
> index 517c09d..28d4e23 100644
> --- a/drivers/net/ethernet/cadence/macb.h
> +++ b/drivers/net/ethernet/cadence/macb.h
> @@ -12,6 +12,7 @@
>  
>  #define MACB_GREGS_NBR 16
>  #define MACB_GREGS_VERSION 1
> +#define MACB_MAX_QUEUES 8
>  
>  /* MACB register offsets */
>  #define MACB_NCR				0x0000
> @@ -88,6 +89,48 @@
>  #define GEM_DCFG5				0x0290
>  #define GEM_DCFG6				0x0294
>  #define GEM_DCFG7				0x0298
> +#define GEM_ISR1				0x0400
> +#define GEM_ISR2				0x0404
> +#define GEM_ISR3				0x0408
> +#define GEM_ISR4				0x040c
> +#define GEM_ISR5				0x0410
> +#define GEM_ISR6				0x0414
> +#define GEM_ISR7				0x0418
> +#define GEM_TBQP1				0x0440
> +#define GEM_TBQP2				0x0444
> +#define GEM_TBQP3				0x0448
> +#define GEM_TBQP4				0x044c
> +#define GEM_TBQP5				0x0450
> +#define GEM_TBQP6				0x0454
> +#define GEM_TBQP7				0x0458
> +#define GEM_RBQP1				0x0480
> +#define GEM_RBQP2				0x0484
> +#define GEM_RBQP3				0x0488
> +#define GEM_RBQP4				0x048c
> +#define GEM_RBQP5				0x0490
> +#define GEM_RBQP6				0x0494
> +#define GEM_RBQP7				0x0498
> +#define GEM_IER1				0x0600
> +#define GEM_IER2				0x0604
> +#define GEM_IER3				0x0608
> +#define GEM_IER4				0x060c
> +#define GEM_IER5				0x0610
> +#define GEM_IER6				0x0614
> +#define GEM_IER7				0x0618
> +#define GEM_IDR1				0x0620
> +#define GEM_IDR2				0x0624
> +#define GEM_IDR3				0x0628
> +#define GEM_IDR4				0x062c
> +#define GEM_IDR5				0x0630
> +#define GEM_IDR6				0x0634
> +#define GEM_IDR7				0x0638
> +#define GEM_IMR1				0x0640
> +#define GEM_IMR2				0x0644
> +#define GEM_IMR3				0x0648
> +#define GEM_IMR4				0x064c
> +#define GEM_IMR5				0x0650
> +#define GEM_IMR6				0x0654
> +#define GEM_IMR7				0x0658
>  
>  /* Bitfields in NCR */
>  #define MACB_LB_OFFSET				0
> @@ -376,6 +419,10 @@
>  	__raw_readl((port)->regs + GEM_##reg)
>  #define gem_writel(port, reg, value)			\
>  	__raw_writel((value), (port)->regs + GEM_##reg)
> +#define queue_readl(queue, reg)				\
> +	__raw_readl((queue)->bp->regs + queue->reg)
> +#define queue_writel(queue, reg, value)			\
> +	__raw_writel((value), (queue)->bp->regs + queue->reg)
>  
>  /*
>   * Conditional GEM/MACB macros.  These perform the operation to the correct
> @@ -597,6 +644,23 @@ struct macb_config {
>  	unsigned int		dma_burst_length;
>  };
>  
> +struct macb_queue {
> +	struct macb		*bp;
> +	int			irq;
> +
> +	unsigned int		ISR;
> +	unsigned int		IER;
> +	unsigned int		IDR;
> +	unsigned int		IMR;
> +	unsigned int		TBQP;
> +
> +	unsigned int		tx_head, tx_tail;
> +	struct macb_dma_desc	*tx_ring;
> +	struct macb_tx_skb	*tx_skb;
> +	dma_addr_t		tx_ring_dma;
> +	struct work_struct	tx_error_task;
> +};
> +
>  struct macb {
>  	void __iomem		*regs;
>  
> @@ -607,9 +671,8 @@ struct macb {
>  	void			*rx_buffers;
>  	size_t			rx_buffer_size;
>  
> -	unsigned int		tx_head, tx_tail;
> -	struct macb_dma_desc	*tx_ring;
> -	struct macb_tx_skb	*tx_skb;
> +	unsigned int		num_queues;
> +	struct macb_queue	queues[MACB_MAX_QUEUES];
>  
>  	spinlock_t		lock;
>  	struct platform_device	*pdev;
> @@ -618,7 +681,6 @@ struct macb {
>  	struct clk		*tx_clk;
>  	struct net_device	*dev;
>  	struct napi_struct	napi;
> -	struct work_struct	tx_error_task;
>  	struct net_device_stats	stats;
>  	union {
>  		struct macb_stats	macb;
> @@ -626,7 +688,6 @@ struct macb {
>  	}			hw_stats;
>  
>  	dma_addr_t		rx_ring_dma;
> -	dma_addr_t		tx_ring_dma;
>  	dma_addr_t		rx_buffers_dma;
>  
>  	struct macb_or_gem_ops	macbgem_ops;
>