gianfar: Add flow control support

[deliverable/linux.git] / drivers / net / ethernet / freescale / gianfar.c

diff --git a/drivers/net/ethernet/freescale/gianfar.c b/drivers/net/ethernet/freescale/gianfar.c
index 3cb464780777cbcbe7b2be4567ed381150d58f0b..b2c91dcd245f7985b66da22346a9b6f7ec5f3fb8 100644 (file)
--- a/drivers/net/ethernet/freescale/gianfar.c
+++ b/drivers/net/ethernet/freescale/gianfar.c
@@ -1016,7 +1016,14 @@ static int gfar_probe(struct platform_device *ofdev)
        /* We need to delay at least 3 TX clocks */
        udelay(2);
 
-       tempval = (MACCFG1_TX_FLOW | MACCFG1_RX_FLOW);
+       tempval = 0;
+       if (!priv->pause_aneg_en && priv->tx_pause_en)
+               tempval |= MACCFG1_TX_FLOW;
+       if (!priv->pause_aneg_en && priv->rx_pause_en)
+               tempval |= MACCFG1_RX_FLOW;
+       /* the soft reset bit is not self-resetting, so we need to
+        * clear it before resuming normal operation
+        */
        gfar_write(&regs->maccfg1, tempval);
 
        /* Initialize MACCFG2. */
@@ -1460,7 +1467,7 @@ static int init_phy(struct net_device *dev)
        struct gfar_private *priv = netdev_priv(dev);
        uint gigabit_support =
                priv->device_flags & FSL_GIANFAR_DEV_HAS_GIGABIT ?
-               SUPPORTED_1000baseT_Full : 0;
+               GFAR_SUPPORTED_GBIT : 0;
        phy_interface_t interface;
 
        priv->oldlink = 0;
@@ -3023,6 +3030,41 @@ static irqreturn_t gfar_interrupt(int irq, void *grp_id)
        return IRQ_HANDLED;
 }
 
+static u32 gfar_get_flowctrl_cfg(struct gfar_private *priv)
+{
+       struct phy_device *phydev = priv->phydev;
+       u32 val = 0;
+
+       if (!phydev->duplex)
+               return val;
+
+       if (!priv->pause_aneg_en) {
+               if (priv->tx_pause_en)
+                       val |= MACCFG1_TX_FLOW;
+               if (priv->rx_pause_en)
+                       val |= MACCFG1_RX_FLOW;
+       } else {
+               u16 lcl_adv, rmt_adv;
+               u8 flowctrl;
+               /* get link partner capabilities */
+               rmt_adv = 0;
+               if (phydev->pause)
+                       rmt_adv = LPA_PAUSE_CAP;
+               if (phydev->asym_pause)
+                       rmt_adv |= LPA_PAUSE_ASYM;
+
+               lcl_adv = mii_advertise_flowctrl(phydev->advertising);
+
+               flowctrl = mii_resolve_flowctrl_fdx(lcl_adv, rmt_adv);
+               if (flowctrl & FLOW_CTRL_TX)
+                       val |= MACCFG1_TX_FLOW;
+               if (flowctrl & FLOW_CTRL_RX)
+                       val |= MACCFG1_RX_FLOW;
+       }
+
+       return val;
+}
+
 /* Called every time the controller might need to be made
  * aware of new link state.  The PHY code conveys this
  * information through variables in the phydev structure, and this
@@ -3041,6 +3083,7 @@ static void adjust_link(struct net_device *dev)
        lock_tx_qs(priv);
 
        if (phydev->link) {
+               u32 tempval1 = gfar_read(&regs->maccfg1);
                u32 tempval = gfar_read(&regs->maccfg2);
                u32 ecntrl = gfar_read(&regs->ecntrl);
 
@@ -3089,6 +3132,10 @@ static void adjust_link(struct net_device *dev)
                        priv->oldspeed = phydev->speed;
                }
 
+               tempval1 &= ~(MACCFG1_TX_FLOW | MACCFG1_RX_FLOW);
+               tempval1 |= gfar_get_flowctrl_cfg(priv);
+
+               gfar_write(&regs->maccfg1, tempval1);
                gfar_write(&regs->maccfg2, tempval);
                gfar_write(&regs->ecntrl, ecntrl);