1 files changed, 103 insertions, 1 deletions

diff --git a/drivers/net/mv643xx_eth.c b/drivers/net/mv643xx_eth.c
index cf18419f96ab..9ce7be09e295 100644
--- a/drivers/net/mv643xx_eth.c
+++ b/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;