mv643xx_eth: add tx rate control

Add an interface for the hardware's per-port and per-subqueue
TX rate control.  In this stage, this is mainly so that we can
disable the bandwidth limits during initialisation of the port.
Signed-off-by: Lennert Buytenhek <buytenh@marvell.com>
Acked-by: Dale Farnsworth <dale@farnsworth.org>

drivers/net/mv643xx_eth.c

@@ -91,7 +91,10 @@ static char mv643xx_eth_driver_version[] = "1.0";
 #define PORT_STATUS(p)			(0x0444 + ((p) << 10))
 #define  TX_FIFO_EMPTY			0x00000400
 #define TXQ_COMMAND(p)			(0x0448 + ((p) << 10))
+#define TXQ_FIX_PRIO_CONF(p)		(0x044c + ((p) << 10))
+#define TX_BW_RATE(p)			(0x0450 + ((p) << 10))
 #define TX_BW_MTU(p)			(0x0458 + ((p) << 10))
+#define TX_BW_BURST(p)			(0x045c + ((p) << 10))
 #define INT_CAUSE(p)			(0x0460 + ((p) << 10))
 #define  INT_RX				0x00000804
 #define  INT_EXT			0x00000002
@@ -107,6 +110,9 @@ static char mv643xx_eth_driver_version[] = "1.0";
 #define RXQ_CURRENT_DESC_PTR(p)		(0x060c + ((p) << 10))
 #define RXQ_COMMAND(p)			(0x0680 + ((p) << 10))
 #define TXQ_CURRENT_DESC_PTR(p)		(0x06c0 + ((p) << 10))
+#define TXQ_BW_TOKENS(p)		(0x0700 + ((p) << 10))
+#define TXQ_BW_CONF(p)			(0x0704 + ((p) << 10))
+#define TXQ_BW_WRR_CONF(p)		(0x0708 + ((p) << 10))
 #define MIB_COUNTERS(p)			(0x1000 + ((p) << 7))
 #define SPECIAL_MCAST_TABLE(p)		(0x1400 + ((p) << 10))
 #define OTHER_MCAST_TABLE(p)		(0x1500 + ((p) << 10))
@@ -775,6 +781,95 @@ static int mv643xx_eth_xmit(struct sk_buff *skb, struct net_device *dev)
 }
 
 
+/* tx rate control **********************************************************/
+/*
+ * Set total maximum TX rate (shared by all TX queues for this port)
+ * to 'rate' bits per second, with a maximum burst of 'burst' bytes.
+ */
+static void tx_set_rate(struct mv643xx_eth_private *mp, int rate, int burst)
+{
+	int token_rate;
+	int mtu;
+	int bucket_size;
+
+	token_rate = ((rate / 1000) * 64) / (mp->shared->t_clk / 1000);
+	if (token_rate > 1023)
+		token_rate = 1023;
+
+	mtu = (mp->dev->mtu + 255) >> 8;
+	if (mtu > 63)
+		mtu = 63;
+
+	bucket_size = (burst + 255) >> 8;
+	if (bucket_size > 65535)
+		bucket_size = 65535;
+
+	wrl(mp, TX_BW_RATE(mp->port_num), token_rate);
+	wrl(mp, TX_BW_MTU(mp->port_num), mtu);
+	wrl(mp, TX_BW_BURST(mp->port_num), bucket_size);
+}
+
+static void txq_set_rate(struct tx_queue *txq, int rate, int burst)
+{
+	struct mv643xx_eth_private *mp = txq_to_mp(txq);
+	int token_rate;
+	int bucket_size;
+
+	token_rate = ((rate / 1000) * 64) / (mp->shared->t_clk / 1000);
+	if (token_rate > 1023)
+		token_rate = 1023;
+
+	bucket_size = (burst + 255) >> 8;
+	if (bucket_size > 65535)
+		bucket_size = 65535;
+
+	wrl(mp, TXQ_BW_TOKENS(mp->port_num), token_rate << 14);
+	wrl(mp, TXQ_BW_CONF(mp->port_num),
+			(bucket_size << 10) | token_rate);
+}
+
+static void txq_set_fixed_prio_mode(struct tx_queue *txq)
+{
+	struct mv643xx_eth_private *mp = txq_to_mp(txq);
+	int off;
+	u32 val;
+
+	/*
+	 * Turn on fixed priority mode.
+	 */
+	off = TXQ_FIX_PRIO_CONF(mp->port_num);
+
+	val = rdl(mp, off);
+	val |= 1;
+	wrl(mp, off, val);
+}
+
+static void txq_set_wrr(struct tx_queue *txq, int weight)
+{
+	struct mv643xx_eth_private *mp = txq_to_mp(txq);
+	int off;
+	u32 val;
+
+	/*
+	 * Turn off fixed priority mode.
+	 */
+	off = TXQ_FIX_PRIO_CONF(mp->port_num);
+
+	val = rdl(mp, off);
+	val &= ~1;
+	wrl(mp, off, val);
+
+	/*
+	 * Configure WRR weight for this queue.
+	 */
+	off = TXQ_BW_WRR_CONF(mp->port_num);
+
+	val = rdl(mp, off);
+	val = (val & ~0xff) | (weight & 0xff);
+	wrl(mp, off, val);
+}
+
+
 /* mii management interface *************************************************/
 #define SMI_BUSY		0x10000000
 #define SMI_READ_VALID		0x08000000
@@ -1581,7 +1676,7 @@ static void port_start(struct mv643xx_eth_private *mp)
 	/*
 	 * Configure TX path and queues.
 	 */
-	wrl(mp, TX_BW_MTU(mp->port_num), 0);
+	tx_set_rate(mp, 1000000000, 16777216);
 	for (i = 0; i < 1; i++) {
 		struct tx_queue *txq = mp->txq;
 		int off = TXQ_CURRENT_DESC_PTR(mp->port_num);
@@ -1590,6 +1685,9 @@ static void port_start(struct mv643xx_eth_private *mp)
 		addr = (u32)txq->tx_desc_dma;
 		addr += txq->tx_curr_desc * sizeof(struct tx_desc);
 		wrl(mp, off, addr);
+
+		txq_set_rate(txq, 1000000000, 16777216);
+		txq_set_fixed_prio_mode(txq);
 	}
 
 	/*
@@ -1749,10 +1847,14 @@ static int mv643xx_eth_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 
 static int mv643xx_eth_change_mtu(struct net_device *dev, int new_mtu)
 {
+	struct mv643xx_eth_private *mp = netdev_priv(dev);
+
 	if (new_mtu < 64 || new_mtu > 9500)
 		return -EINVAL;
 
 	dev->mtu = new_mtu;
+	tx_set_rate(mp, 1000000000, 16777216);
+
 	if (!netif_running(dev))
 		return 0;