net: bcmasp: Add support for eee mode

Add support for eee mode.

Signed-off-by: Justin Chen <justin.chen@broadcom.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/net/ethernet/broadcom/asp2/bcmasp.h b/drivers/net/ethernet/broadcom/asp2/bcmasp.h
index fbbde04..18a4404 100644 (file)
--- a/drivers/net/ethernet/broadcom/asp2/bcmasp.h
+++ b/drivers/net/ethernet/broadcom/asp2/bcmasp.h
@@ -315,6 +315,8 @@ struct bcmasp_intf {
        /* Used if per intf wol irq */
        int                             wol_irq;
        unsigned int                    wol_irq_enabled:1;
+
+       struct ethtool_eee              eee;
 };
 
 #define NUM_NET_FILTERS                                32
@@ -558,4 +560,6 @@ void bcmasp_netfilt_get_all_active(struct bcmasp_intf *intf, u32 *rule_locs,
                                   u32 *rule_cnt);
 
 void bcmasp_netfilt_suspend(struct bcmasp_intf *intf);
+
+void bcmasp_eee_enable_set(struct bcmasp_intf *intf, bool enable);
 #endif

diff --git a/drivers/net/ethernet/broadcom/asp2/bcmasp_ethtool.c b/drivers/net/ethernet/broadcom/asp2/bcmasp_ethtool.c
index eddd1c4..1e96a69 100644 (file)
--- a/drivers/net/ethernet/broadcom/asp2/bcmasp_ethtool.c
+++ b/drivers/net/ethernet/broadcom/asp2/bcmasp_ethtool.c
@@ -191,6 +191,65 @@ static int bcmasp_get_rxnfc(struct net_device *dev, struct ethtool_rxnfc *cmd,
        return err;
 }
 
+void bcmasp_eee_enable_set(struct bcmasp_intf *intf, bool enable)
+{
+       u32 reg;
+
+       reg = umac_rl(intf, UMC_EEE_CTRL);
+       if (enable)
+               reg |= EEE_EN;
+       else
+               reg &= ~EEE_EN;
+       umac_wl(intf, reg, UMC_EEE_CTRL);
+
+       intf->eee.eee_enabled = enable;
+       intf->eee.eee_active = enable;
+}
+
+static int bcmasp_get_eee(struct net_device *dev, struct ethtool_eee *e)
+{
+       struct bcmasp_intf *intf = netdev_priv(dev);
+       struct ethtool_eee *p = &intf->eee;
+
+       if (!dev->phydev)
+               return -ENODEV;
+
+       e->eee_enabled = p->eee_enabled;
+       e->eee_active = p->eee_active;
+       e->tx_lpi_enabled = p->tx_lpi_enabled;
+       e->tx_lpi_timer = umac_rl(intf, UMC_EEE_LPI_TIMER);
+
+       return phy_ethtool_get_eee(dev->phydev, e);
+}
+
+static int bcmasp_set_eee(struct net_device *dev, struct ethtool_eee *e)
+{
+       struct bcmasp_intf *intf = netdev_priv(dev);
+       struct ethtool_eee *p = &intf->eee;
+       int ret;
+
+       if (!dev->phydev)
+               return -ENODEV;
+
+       if (!p->eee_enabled) {
+               bcmasp_eee_enable_set(intf, false);
+       } else {
+               ret = phy_init_eee(dev->phydev, 0);
+               if (ret) {
+                       netif_err(intf, hw, dev,
+                                 "EEE initialization failed: %d\n", ret);
+                       return ret;
+               }
+
+               umac_wl(intf, e->tx_lpi_timer, UMC_EEE_LPI_TIMER);
+               intf->eee.eee_active = ret >= 0;
+               intf->eee.tx_lpi_enabled = e->tx_lpi_enabled;
+               bcmasp_eee_enable_set(intf, true);
+       }
+
+       return phy_ethtool_set_eee(dev->phydev, e);
+}
+
 const struct ethtool_ops bcmasp_ethtool_ops = {
        .get_drvinfo            = bcmasp_get_drvinfo,
        .get_link               = ethtool_op_get_link,
@@ -202,4 +261,6 @@ const struct ethtool_ops bcmasp_ethtool_ops = {
        .set_wol                = bcmasp_set_wol,
        .get_rxnfc              = bcmasp_get_rxnfc,
        .set_rxnfc              = bcmasp_set_rxnfc,
+       .set_eee                = bcmasp_set_eee,
+       .get_eee                = bcmasp_get_eee,
 };

diff --git a/drivers/net/ethernet/broadcom/asp2/bcmasp_intf.c b/drivers/net/ethernet/broadcom/asp2/bcmasp_intf.c
index ad84223..051f882 100644 (file)
--- a/drivers/net/ethernet/broadcom/asp2/bcmasp_intf.c
+++ b/drivers/net/ethernet/broadcom/asp2/bcmasp_intf.c
@@ -646,6 +646,9 @@ static void bcmasp_adj_link(struct net_device *dev)
                        UMC_CMD_TX_PAUSE_IGNORE);
                reg |= cmd_bits;
                umac_wl(intf, reg, UMC_CMD);
+
+               intf->eee.eee_active = phy_init_eee(phydev, 0) >= 0;
+               bcmasp_eee_enable_set(intf, intf->eee.eee_active);
        }
 
        reg = rgmii_rl(intf, RGMII_OOB_CNTRL);
@@ -1387,6 +1390,9 @@ int bcmasp_interface_resume(struct bcmasp_intf *intf)
 
        bcmasp_resume_from_wol(intf);
 
+       if (intf->eee.eee_enabled)
+               bcmasp_eee_enable_set(intf, true);
+
        netif_device_attach(dev);
 
        return 0;