net: macb: add gmac multi-queue support

This patch refer to linux kernel commit: d8b763e1e79f
  net/macb: add TX multiqueue support for gem
  by: Cyrille Pitchen

1. macb driver will check the register to find how many queues support for
this chip.

2. Then as we only use queue0 for tx, so we will set up all other queues
use a dummy descriptor, which USED bit is set. So those queues are not used.

Signed-off-by: Josh Wu <josh.wu@atmel.com>
Acked-by: Joe Hershberger <joe.hershberger@ni.com>

2 files changed, 42 insertions, 0 deletions

diff --git a/drivers/net/macb.c b/drivers/net/macb.c
index f949161..a5c1880 100644
--- a/drivers/net/macb.c
+++ b/drivers/net/macb.c
@@ -54,6 +54,7 @@ struct macb_dma_desc {
 #define DMA_DESC_BYTES(n)	(n * sizeof(struct macb_dma_desc))
 #define MACB_TX_DMA_DESC_SIZE	(DMA_DESC_BYTES(MACB_TX_RING_SIZE))
 #define MACB_RX_DMA_DESC_SIZE	(DMA_DESC_BYTES(MACB_RX_RING_SIZE))
+#define MACB_TX_DUMMY_DMA_DESC_SIZE	(DMA_DESC_BYTES(1))
 
 #define RXADDR_USED		0x00000001
 #define RXADDR_WRAP		0x00000002
@@ -93,6 +94,9 @@ struct macb_device {
 	unsigned long		rx_ring_dma;
 	unsigned long		tx_ring_dma;
 
+	struct macb_dma_desc	*dummy_desc;
+	unsigned long		dummy_desc_dma;
+
 	const struct device	*dev;
 	struct eth_device	netdev;
 	unsigned short		phy_addr;
@@ -525,6 +529,30 @@ static int macb_phy_init(struct macb_device *macb)
 	return 1;
 }
 
+static int gmac_init_multi_queues(struct macb_device *macb)
+{
+	int i, num_queues = 1;
+	u32 queue_mask;
+
+	/* bit 0 is never set but queue 0 always exists */
+	queue_mask = gem_readl(macb, DCFG6) & 0xff;
+	queue_mask |= 0x1;
+
+	for (i = 1; i < MACB_MAX_QUEUES; i++)
+		if (queue_mask & (1 << i))
+			num_queues++;
+
+	macb->dummy_desc->ctrl = TXBUF_USED;
+	macb->dummy_desc->addr = 0;
+	flush_dcache_range(macb->dummy_desc_dma, macb->dummy_desc_dma +
+			MACB_TX_DUMMY_DMA_DESC_SIZE);
+
+	for (i = 1; i < num_queues; i++)
+		gem_writel_queue_TBQP(macb, macb->dummy_desc_dma, i - 1);
+
+	return 0;
+}
+
 static int macb_init(struct eth_device *netdev, bd_t *bd)
 {
 	struct macb_device *macb = to_macb(netdev);
@@ -565,6 +593,9 @@ static int macb_init(struct eth_device *netdev, bd_t *bd)
 	macb_writel(macb, TBQP, macb->tx_ring_dma);
 
 	if (macb_is_gem(macb)) {
+		/* Check the multi queue and initialize the queue for tx */
+		gmac_init_multi_queues(macb);
+
 		/*
 		 * When the GMAC IP with GE feature, this bit is used to
 		 * select interface between RGMII and GMII.
@@ -712,6 +743,8 @@ int macb_eth_initialize(int id, void *regs, unsigned int phy_addr)
 					   &macb->rx_ring_dma);
 	macb->tx_ring = dma_alloc_coherent(MACB_TX_DMA_DESC_SIZE,
 					   &macb->tx_ring_dma);
+	macb->dummy_desc = dma_alloc_coherent(MACB_TX_DUMMY_DMA_DESC_SIZE,
+					   &macb->dummy_desc_dma);
 
 	/* TODO: we need check the rx/tx_ring_dma is dcache line aligned */
 
diff --git a/drivers/net/macb.h b/drivers/net/macb.h
index 06f7c66..5bb48f4 100644
--- a/drivers/net/macb.h
+++ b/drivers/net/macb.h
@@ -60,6 +60,13 @@
 
 /* GEM specific register offsets */
 #define GEM_DCFG1				0x0280
+#define GEM_DCFG6				0x0294
+
+#define MACB_MAX_QUEUES				8
+
+/* GEM specific multi queues register offset */
+/* hw_q can be 0~7 */
+#define GEM_TBQP(hw_q)				(0x0440 + ((hw_q) << 2))
 
 /* Bitfields in NCR */
 #define MACB_LB_OFFSET				0
@@ -309,5 +316,7 @@
 	readl((port)->regs + GEM_##reg)
 #define gem_writel(port, reg, value)			\
 	writel((value), (port)->regs + GEM_##reg)
+#define gem_writel_queue_TBQP(port, value, queue_num)	\
+	writel((value), (port)->regs + GEM_TBQP(queue_num))
 
 #endif /* __DRIVERS_MACB_H__ */