r8152: support pauseparam of ethtool_ops

Support get_pauseparam and set_pauseparam of ethtool_ops.

Signed-off-by: Hayes Wang <hayeswang@realtek.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/net/usb/r8152.c b/drivers/net/usb/r8152.c
index f6abb2f..21e6b9b 100644 (file)
--- a/drivers/net/usb/r8152.c
+++ b/drivers/net/usb/r8152.c
@@ -8967,6 +8967,79 @@ static int rtl8152_set_ringparam(struct net_device *netdev,
        return 0;
 }
 
+static void rtl8152_get_pauseparam(struct net_device *netdev, struct ethtool_pauseparam *pause)
+{
+       struct r8152 *tp = netdev_priv(netdev);
+       u16 bmcr, lcladv, rmtadv;
+       u8 cap;
+
+       if (usb_autopm_get_interface(tp->intf) < 0)
+               return;
+
+       mutex_lock(&tp->control);
+
+       bmcr = r8152_mdio_read(tp, MII_BMCR);
+       lcladv = r8152_mdio_read(tp, MII_ADVERTISE);
+       rmtadv = r8152_mdio_read(tp, MII_LPA);
+
+       mutex_unlock(&tp->control);
+
+       usb_autopm_put_interface(tp->intf);
+
+       if (!(bmcr & BMCR_ANENABLE)) {
+               pause->autoneg = 0;
+               pause->rx_pause = 0;
+               pause->tx_pause = 0;
+               return;
+       }
+
+       pause->autoneg = 1;
+
+       cap = mii_resolve_flowctrl_fdx(lcladv, rmtadv);
+
+       if (cap & FLOW_CTRL_RX)
+               pause->rx_pause = 1;
+
+       if (cap & FLOW_CTRL_TX)
+               pause->tx_pause = 1;
+}
+
+static int rtl8152_set_pauseparam(struct net_device *netdev, struct ethtool_pauseparam *pause)
+{
+       struct r8152 *tp = netdev_priv(netdev);
+       u16 old, new1;
+       u8 cap = 0;
+       int ret;
+
+       ret = usb_autopm_get_interface(tp->intf);
+       if (ret < 0)
+               return ret;
+
+       mutex_lock(&tp->control);
+
+       if (pause->autoneg && !(r8152_mdio_read(tp, MII_BMCR) & BMCR_ANENABLE)) {
+               ret = -EINVAL;
+               goto out;
+       }
+
+       if (pause->rx_pause)
+               cap |= FLOW_CTRL_RX;
+
+       if (pause->tx_pause)
+               cap |= FLOW_CTRL_TX;
+
+       old = r8152_mdio_read(tp, MII_ADVERTISE);
+       new1 = (old & ~(ADVERTISE_PAUSE_CAP | ADVERTISE_PAUSE_ASYM)) | mii_advertise_flowctrl(cap);
+       if (old != new1)
+               r8152_mdio_write(tp, MII_ADVERTISE, new1);
+
+out:
+       mutex_unlock(&tp->control);
+       usb_autopm_put_interface(tp->intf);
+
+       return ret;
+}
+
 static const struct ethtool_ops ops = {
        .supported_coalesce_params = ETHTOOL_COALESCE_USECS,
        .get_drvinfo = rtl8152_get_drvinfo,
@@ -8989,6 +9062,8 @@ static const struct ethtool_ops ops = {
        .set_tunable = rtl8152_set_tunable,
        .get_ringparam = rtl8152_get_ringparam,
        .set_ringparam = rtl8152_set_ringparam,
+       .get_pauseparam = rtl8152_get_pauseparam,
+       .set_pauseparam = rtl8152_set_pauseparam,
 };
 
 static int rtl8152_ioctl(struct net_device *netdev, struct ifreq *rq, int cmd)