[PATCH v1 10/18] net/r8169: add link status and interrupt management

[Howard Wang <howard_wang@realsil.com.cn>]

Signed-off-by: Howard Wang <howard_wang@realsil.com.cn>
---
 drivers/net/r8169/r8169_base.h   |   5 +-
 drivers/net/r8169/r8169_ethdev.c | 279 ++++++++++++++++++++++++++++++-
 drivers/net/r8169/r8169_ethdev.h |   3 +
 drivers/net/r8169/r8169_hw.c     |   8 +-
 drivers/net/r8169/r8169_hw.h     |   3 +
 drivers/net/r8169/r8169_phy.c    | 121 ++++++++++++++
 drivers/net/r8169/r8169_phy.h    |   3 +
 7 files changed, 413 insertions(+), 9 deletions(-)

diff --git a/drivers/net/r8169/r8169_base.h b/drivers/net/r8169/r8169_base.h
index 2ee6fc6782..2960288981 100644
--- a/drivers/net/r8169/r8169_base.h
+++ b/drivers/net/r8169/r8169_base.h
@@ -379,6 +379,7 @@ enum RTL_register_content {
 
 	/* PHY status */
 	PowerSaveStatus = 0x80,
+	_5000bpsF       = 0x1000,
 	_2500bpsF       = 0x400,
 	TxFlowCtrl      = 0x40,
 	RxFlowCtrl      = 0x20,
@@ -559,10 +560,6 @@ enum RTL_chipset_name {
 #define ADVERTISE_5000_HALF   0x0100 /* NOT used, just FYI */
 #define ADVERTISE_5000_FULL   0x0200
 
-#define RTL8126_ALL_SPEED_DUPLEX (ADVERTISE_10_HALF | ADVERTISE_10_FULL | \
-        ADVERTISE_100_HALF | ADVERTISE_100_FULL | ADVERTISE_1000_FULL | \
-        ADVERTISE_2500_FULL | ADVERTISE_5000_FULL)
-
 #define MAC_ADDR_LEN    RTE_ETHER_ADDR_LEN
 
 static inline u32
diff --git a/drivers/net/r8169/r8169_ethdev.c b/drivers/net/r8169/r8169_ethdev.c
index 9af46b390c..ecf4a4e984 100644
--- a/drivers/net/r8169/r8169_ethdev.c
+++ b/drivers/net/r8169/r8169_ethdev.c
@@ -35,6 +35,9 @@ static int rtl_dev_start(struct rte_eth_dev *dev);
 static int rtl_dev_stop(struct rte_eth_dev *dev);
 static int rtl_dev_reset(struct rte_eth_dev *dev);
 static int rtl_dev_close(struct rte_eth_dev *dev);
+static int rtl_dev_link_update(struct rte_eth_dev *dev, int wait __rte_unused);
+static int rtl_dev_set_link_up(struct rte_eth_dev *dev);
+static int rtl_dev_set_link_down(struct rte_eth_dev *dev);
 
 /*
  * The set of PCI devices this driver supports
@@ -53,6 +56,10 @@ static const struct eth_dev_ops rtl_eth_dev_ops = {
 	.dev_stop	      = rtl_dev_stop,
 	.dev_close	      = rtl_dev_close,
 	.dev_reset	      = rtl_dev_reset,
+	.dev_set_link_up      = rtl_dev_set_link_up,
+	.dev_set_link_down    = rtl_dev_set_link_down,
+
+	.link_update          = rtl_dev_link_update,
 };
 
 static int
@@ -61,6 +68,119 @@ rtl_dev_configure(struct rte_eth_dev *dev __rte_unused)
 	return 0;
 }
 
+static void
+rtl_disable_intr(struct rtl_hw *hw)
+{
+	PMD_INIT_FUNC_TRACE();
+	RTL_W32(hw, IMR0_8125, 0x0000);
+	RTL_W32(hw, ISR0_8125, RTL_R32(hw, ISR0_8125));
+}
+
+static void
+rtl_enable_intr(struct rtl_hw *hw)
+{
+	PMD_INIT_FUNC_TRACE();
+	RTL_W32(hw, IMR0_8125, LinkChg);
+}
+
+static int
+_rtl_setup_link(struct rte_eth_dev *dev)
+{
+	struct rtl_adapter *adapter = RTL_DEV_PRIVATE(dev);
+	struct rtl_hw *hw = &adapter->hw;
+	u64 adv = 0;
+	u32 *link_speeds = &dev->data->dev_conf.link_speeds;
+
+	/* Setup link speed and duplex */
+	if (*link_speeds == RTE_ETH_LINK_SPEED_AUTONEG)
+		rtl_set_link_option(hw, AUTONEG_ENABLE, SPEED_5000, DUPLEX_FULL, rtl_fc_full);
+	else if (*link_speeds != 0) {
+
+		if (*link_speeds & ~(RTE_ETH_LINK_SPEED_10M_HD | RTE_ETH_LINK_SPEED_10M |
+		                     RTE_ETH_LINK_SPEED_100M_HD | RTE_ETH_LINK_SPEED_100M |
+		                     RTE_ETH_LINK_SPEED_1G | RTE_ETH_LINK_SPEED_2_5G |
+		                     RTE_ETH_LINK_SPEED_5G | RTE_ETH_LINK_SPEED_FIXED))
+			goto error_invalid_config;
+
+		if (*link_speeds & RTE_ETH_LINK_SPEED_10M_HD) {
+			hw->speed = SPEED_10;
+			hw->duplex = DUPLEX_HALF;
+			adv |= ADVERTISE_10_HALF;
+		}
+		if (*link_speeds & RTE_ETH_LINK_SPEED_10M) {
+			hw->speed = SPEED_10;
+			hw->duplex = DUPLEX_FULL;
+			adv |= ADVERTISE_10_FULL;
+		}
+		if (*link_speeds & RTE_ETH_LINK_SPEED_100M_HD) {
+			hw->speed = SPEED_100;
+			hw->duplex = DUPLEX_HALF;
+			adv |= ADVERTISE_100_HALF;
+		}
+		if (*link_speeds & RTE_ETH_LINK_SPEED_100M) {
+			hw->speed = SPEED_100;
+			hw->duplex = DUPLEX_FULL;
+			adv |= ADVERTISE_100_FULL;
+		}
+		if (*link_speeds & RTE_ETH_LINK_SPEED_1G) {
+			hw->speed = SPEED_1000;
+			hw->duplex = DUPLEX_FULL;
+			adv |= ADVERTISE_1000_FULL;
+		}
+		if (*link_speeds & RTE_ETH_LINK_SPEED_2_5G) {
+			hw->speed = SPEED_2500;
+			hw->duplex = DUPLEX_FULL;
+			adv |= ADVERTISE_2500_FULL;
+		}
+		if (*link_speeds & RTE_ETH_LINK_SPEED_5G) {
+			hw->speed = SPEED_5000;
+			hw->duplex = DUPLEX_FULL;
+			adv |= ADVERTISE_5000_FULL;
+		}
+
+		hw->autoneg = AUTONEG_ENABLE;
+		hw->advertising = adv;
+	}
+
+	rtl_set_speed(hw);
+
+	return 0;
+
+error_invalid_config:
+	PMD_INIT_LOG(ERR, "Invalid advertised speeds (%u) for port %u",
+	             dev->data->dev_conf.link_speeds, dev->data->port_id);
+	return -EINVAL;
+}
+
+static int
+rtl_setup_link(struct rte_eth_dev *dev)
+{
+#ifdef RTE_EXEC_ENV_FREEBSD
+	struct rtl_adapter *adapter = RTL_DEV_PRIVATE(dev);
+	struct rtl_hw *hw = &adapter->hw;
+	struct rte_eth_link link;
+	int count;
+#endif
+
+	_rtl_setup_link(dev);
+
+#ifdef RTE_EXEC_ENV_FREEBSD
+	for (count = 0; count < R8169_LINK_CHECK_TIMEOUT; count++) {
+		if (!(RTL_R16(hw, PHYstatus) & LinkStatus)) {
+			msleep(R8169_LINK_CHECK_INTERVAL);
+			continue;
+		}
+
+		rtl_dev_link_update(dev, 0);
+
+		rte_eth_linkstatus_get(dev, &link);
+
+		return 0;
+	}
+#endif
+	return 0;
+}
+
 /*
  * Configure device link speed and setup link.
  * It returns 0 on success.
@@ -70,8 +190,13 @@ rtl_dev_start(struct rte_eth_dev *dev)
 {
 	struct rtl_adapter *adapter = RTL_DEV_PRIVATE(dev);
 	struct rtl_hw *hw = &adapter->hw;
+	struct rte_pci_device *pci_dev = RTE_ETH_DEV_TO_PCI(dev);
+	struct rte_intr_handle *intr_handle = pci_dev->intr_handle;
 	int err;
 
+	/* Disable uio/vfio intr/eventfd mapping */
+	rte_intr_disable(intr_handle);
+
 	rtl_powerup_pll(hw);
 
 	rtl_hw_ephy_config(hw);
@@ -90,6 +215,14 @@ rtl_dev_start(struct rte_eth_dev *dev)
 		goto error;
 	}
 
+	/* Enable uio/vfio intr/eventfd mapping */
+	rte_intr_enable(intr_handle);
+
+	/* Resume enabled intr since hw reset */
+	rtl_enable_intr(hw);
+
+	rtl_setup_link(dev);
+
 	rtl_mdio_write(hw, 0x1F, 0x0000);
 
 	hw->adapter_stopped = 0;
@@ -107,10 +240,13 @@ rtl_dev_stop(struct rte_eth_dev *dev)
 {
 	struct rtl_adapter *adapter = RTL_DEV_PRIVATE(dev);
 	struct rtl_hw *hw = &adapter->hw;
+	struct rte_eth_link link;
 
 	if (hw->adapter_stopped)
 		return 0;
 
+	rtl_disable_intr(hw);
+
 	rtl_nic_reset(hw);
 
 	switch (hw->mcfg) {
@@ -122,21 +258,140 @@ rtl_dev_stop(struct rte_eth_dev *dev)
 
 	rtl_powerdown_pll(hw);
 
+	/* Clear the recorded link status */
+	memset(&link, 0, sizeof(link));
+	rte_eth_linkstatus_set(dev, &link);
+
 	hw->adapter_stopped = 1;
 	dev->data->dev_started = 0;
 
 	return 0;
 }
 
+static int
+rtl_dev_set_link_up(struct rte_eth_dev *dev)
+{
+	struct rtl_adapter *adapter = RTL_DEV_PRIVATE(dev);
+	struct rtl_hw *hw = &adapter->hw;
+
+	rtl_powerup_pll(hw);
+
+	return 0;
+}
+
+static int
+rtl_dev_set_link_down(struct rte_eth_dev *dev)
+{
+	struct rtl_adapter *adapter = RTL_DEV_PRIVATE(dev);
+	struct rtl_hw *hw = &adapter->hw;
+
+	/* mcu pme intr masks */
+	switch (hw->mcfg) {
+	case CFG_METHOD_48 ... CFG_METHOD_57:
+	case CFG_METHOD_69 ... CFG_METHOD_71:
+		rtl_mac_ocp_write(hw, 0xE00A, hw->mcu_pme_setting & ~(BIT_11 | BIT_14));
+		break;
+	}
+
+	rtl_powerdown_pll(hw);
+
+	return 0;
+}
+
+/* Return 0 means link status changed, -1 means not changed */
+static int
+rtl_dev_link_update(struct rte_eth_dev *dev, int wait __rte_unused)
+{
+	struct rte_eth_link link, old;
+	struct rtl_adapter *adapter = RTL_DEV_PRIVATE(dev);
+	struct rtl_hw *hw = &adapter->hw;
+	u32 speed;
+	u16 status;
+
+	link.link_status = RTE_ETH_LINK_DOWN;
+	link.link_speed = 0;
+	link.link_duplex = RTE_ETH_LINK_FULL_DUPLEX;
+	link.link_autoneg = RTE_ETH_LINK_AUTONEG;
+
+	memset(&old, 0, sizeof(old));
+
+	/* Load old link status */
+	rte_eth_linkstatus_get(dev, &old);
+
+	/* Read current link status */
+	status = RTL_R16(hw, PHYstatus);
+
+	if (status & LinkStatus) {
+		link.link_status = RTE_ETH_LINK_UP;
+
+		if (status & FullDup) {
+			link.link_duplex = RTE_ETH_LINK_FULL_DUPLEX;
+			if (hw->mcfg == CFG_METHOD_2)
+				RTL_W32(hw, TxConfig, (RTL_R32(hw, TxConfig) |
+				                       (BIT_24 | BIT_25)) & ~BIT_19);
+
+		} else {
+			link.link_duplex = RTE_ETH_LINK_HALF_DUPLEX;
+			if (hw->mcfg == CFG_METHOD_2)
+				RTL_W32(hw, TxConfig, (RTL_R32(hw, TxConfig) | BIT_25) &
+				        ~(BIT_19 | BIT_24));
+		}
+
+		if (status & _5000bpsF)
+			speed = 5000;
+		else if (status & _2500bpsF)
+			speed = 2500;
+		else if (status & _1000bpsF)
+			speed = 1000;
+		else if (status & _100bps)
+			speed = 100;
+		else
+			speed = 10;
+
+		link.link_speed = speed;
+	}
+
+	if (link.link_status == old.link_status)
+		return -1;
+
+	rte_eth_linkstatus_set(dev, &link);
+
+	return 0;
+}
+
+static void
+rtl_dev_interrupt_handler(void *param)
+{
+	struct rte_eth_dev *dev = (struct rte_eth_dev *)param;
+	struct rtl_adapter *adapter = RTL_DEV_PRIVATE(dev);
+	struct rtl_hw *hw = &adapter->hw;
+	uint32_t intr;
+
+	intr = RTL_R32(hw, ISR0_8125);
+
+	/* Clear all cause mask */
+	rtl_disable_intr(hw);
+
+	if (intr & LinkChg)
+		rtl_dev_link_update(dev, 0);
+	else
+		PMD_DRV_LOG(ERR, "r8169: interrupt unhandled.");
+
+	rtl_enable_intr(hw);
+}
+
 /*
  * Reset and stop device.
  */
 static int
 rtl_dev_close(struct rte_eth_dev *dev)
 {
+	struct rte_pci_device *pci_dev = RTE_ETH_DEV_TO_PCI(dev);
+	struct rte_intr_handle *intr_handle = pci_dev->intr_handle;
 	struct rtl_adapter *adapter = RTL_DEV_PRIVATE(dev);
 	struct rtl_hw *hw = &adapter->hw;
-	int ret_stp;
+	int retries = 0;
+	int ret_unreg, ret_stp;
 
 	if (rte_eal_process_type() != RTE_PROC_PRIMARY)
 		return 0;
@@ -146,6 +401,20 @@ rtl_dev_close(struct rte_eth_dev *dev)
 	/* Reprogram the RAR[0] in case user changed it. */
 	rtl_rar_set(hw, hw->mac_addr);
 
+	/* Disable uio intr before callback unregister */
+	rte_intr_disable(intr_handle);
+
+	do {
+		ret_unreg = rte_intr_callback_unregister(intr_handle, rtl_dev_interrupt_handler,
+		                dev);
+		if (ret_unreg >= 0 || ret_unreg == -ENOENT)
+			break;
+		else if (ret_unreg != -EAGAIN)
+			PMD_DRV_LOG(ERR, "r8169: intr callback unregister failed: %d", ret_unreg);
+
+		rte_delay_ms(100);
+	} while (retries++ < (10 + 90));
+
 	return ret_stp;
 }
 
@@ -153,6 +422,7 @@ static int
 rtl_dev_init(struct rte_eth_dev *dev)
 {
 	struct rte_pci_device *pci_dev = RTE_ETH_DEV_TO_PCI(dev);
+	struct rte_intr_handle *intr_handle = pci_dev->intr_handle;
 	struct rtl_adapter *adapter = RTL_DEV_PRIVATE(dev);
 	struct rtl_hw *hw = &adapter->hw;
 	struct rte_ether_addr *perm_addr = (struct rte_ether_addr *)hw->mac_addr;
@@ -175,6 +445,8 @@ rtl_dev_init(struct rte_eth_dev *dev)
 	if (rtl_set_hw_ops(hw))
 		return -ENOTSUP;
 
+	rtl_disable_intr(hw);
+
 	rtl_hw_initialize(hw);
 
 	/* Read the permanent MAC address out of ROM */
@@ -201,6 +473,11 @@ rtl_dev_init(struct rte_eth_dev *dev)
 
 	rtl_rar_set(hw, &perm_addr->addr_bytes[0]);
 
+	rte_intr_callback_register(intr_handle, rtl_dev_interrupt_handler, dev);
+
+	/* Enable uio/vfio intr/eventfd mapping */
+	rte_intr_enable(intr_handle);
+
 	return 0;
 }
 
diff --git a/drivers/net/r8169/r8169_ethdev.h b/drivers/net/r8169/r8169_ethdev.h
index de90b33289..03012365ca 100644
--- a/drivers/net/r8169/r8169_ethdev.h
+++ b/drivers/net/r8169/r8169_ethdev.h
@@ -101,6 +101,9 @@ struct rtl_adapter {
 #define RTL_DEV_PRIVATE(eth_dev) \
 	((struct rtl_adapter *)((eth_dev)->data->dev_private))
 
+#define R8169_LINK_CHECK_TIMEOUT  50   /* 10s */
+#define R8169_LINK_CHECK_INTERVAL 200  /* ms */
+
 int rtl_rx_init(struct rte_eth_dev *dev);
 int rtl_tx_init(struct rte_eth_dev *dev);
 
diff --git a/drivers/net/r8169/r8169_hw.c b/drivers/net/r8169/r8169_hw.c
index e5d1b249e2..3be56061cf 100644
--- a/drivers/net/r8169/r8169_hw.c
+++ b/drivers/net/r8169/r8169_hw.c
@@ -960,7 +960,7 @@ rtl_is_autoneg_mode_valid(u32 autoneg)
 	}
 }
 
-static void
+void
 rtl_set_link_option(struct rtl_hw *hw, u8 autoneg, u32 speed, u8 duplex,
                     enum rtl_fc_mode fc)
 {
@@ -1090,13 +1090,13 @@ rtl_init_software_variable(struct rtl_hw *hw)
 	switch (hw->mcfg) {
 	case CFG_METHOD_48 ... CFG_METHOD_51:
 	case CFG_METHOD_54 ... CFG_METHOD_57:
-		hw->HwSuppMaxPhyLinkSpeed = 2500;
+		hw->HwSuppMaxPhyLinkSpeed = SPEED_2500;
 		break;
 	case CFG_METHOD_69 ... CFG_METHOD_71:
-		hw->HwSuppMaxPhyLinkSpeed = 5000;
+		hw->HwSuppMaxPhyLinkSpeed = SPEED_5000;
 		break;
 	default:
-		hw->HwSuppMaxPhyLinkSpeed = 1000;
+		hw->HwSuppMaxPhyLinkSpeed = SPEED_1000;
 		break;
 	}
 
diff --git a/drivers/net/r8169/r8169_hw.h b/drivers/net/r8169/r8169_hw.h
index 3746b46084..857f71deac 100644
--- a/drivers/net/r8169/r8169_hw.h
+++ b/drivers/net/r8169/r8169_hw.h
@@ -51,6 +51,9 @@ int rtl_get_mac_address(struct rtl_hw *hw, struct rte_ether_addr *ea);
 
 void rtl_rar_set(struct rtl_hw *hw, uint8_t *addr);
 
+void rtl_set_link_option(struct rtl_hw *hw, u8 autoneg, u32 speed, u8 duplex,
+                         enum rtl_fc_mode fc);
+
 extern const struct rtl_hw_ops rtl8125a_ops;
 extern const struct rtl_hw_ops rtl8125b_ops;
 extern const struct rtl_hw_ops rtl8125bp_ops;
diff --git a/drivers/net/r8169/r8169_phy.c b/drivers/net/r8169/r8169_phy.c
index 3198116946..da033de79e 100644
--- a/drivers/net/r8169/r8169_phy.c
+++ b/drivers/net/r8169/r8169_phy.c
@@ -775,3 +775,124 @@ rtl_hw_phy_config(struct rtl_hw *hw)
 		rtl_disable_eee(hw);
 }
 
+static void
+rtl_phy_restart_nway(struct rtl_hw *hw)
+{
+	if (rtl_is_in_phy_disable_mode(hw))
+		return;
+
+	rtl_mdio_write(hw, 0x1F, 0x0000);
+	rtl_mdio_write(hw, MII_BMCR, BMCR_ANENABLE | BMCR_ANRESTART);
+}
+
+static void
+rtl_phy_setup_force_mode(struct rtl_hw *hw, u32 speed, u8 duplex)
+{
+	u16 bmcr_true_force = 0;
+
+	if (rtl_is_in_phy_disable_mode(hw))
+		return;
+
+	if (speed == SPEED_10 && duplex == DUPLEX_HALF)
+		bmcr_true_force = BMCR_SPEED10;
+	else if (speed == SPEED_10 && duplex == DUPLEX_FULL)
+		bmcr_true_force = BMCR_SPEED10 | BMCR_FULLDPLX;
+	else if (speed == SPEED_100 && duplex == DUPLEX_HALF)
+		bmcr_true_force = BMCR_SPEED100;
+	else if (speed == SPEED_100 && duplex == DUPLEX_FULL)
+		bmcr_true_force = BMCR_SPEED100 | BMCR_FULLDPLX;
+	else
+		return;
+
+	rtl_mdio_write(hw, 0x1F, 0x0000);
+	rtl_mdio_write(hw, MII_BMCR, bmcr_true_force);
+}
+
+static int
+rtl_set_speed_xmii(struct rtl_hw *hw, u8 autoneg, u32 speed, u8 duplex, u32 adv)
+{
+	int auto_nego = 0;
+	int giga_ctrl = 0;
+	int ctrl_2500 = 0;
+	int rc = -EINVAL;
+
+	/* Disable giga lite */
+	rtl_clear_eth_phy_ocp_bit(hw, 0xA428, BIT_9);
+	rtl_clear_eth_phy_ocp_bit(hw, 0xA5EA, BIT_0);
+
+	if (HW_SUPP_PHY_LINK_SPEED_5000M(hw))
+		rtl_clear_eth_phy_ocp_bit(hw, 0xA5EA, BIT_1);
+
+	if (!rtl_is_speed_mode_valid(speed)) {
+		speed = hw->HwSuppMaxPhyLinkSpeed;
+		duplex = DUPLEX_FULL;
+		adv |= hw->advertising;
+	}
+
+	giga_ctrl = rtl_mdio_read(hw, MII_CTRL1000);
+	giga_ctrl &= ~(ADVERTISE_1000HALF | ADVERTISE_1000FULL);
+	ctrl_2500 = rtl_mdio_direct_read_phy_ocp(hw, 0xA5D4);
+	ctrl_2500 &= ~(RTK_ADVERTISE_2500FULL | RTK_ADVERTISE_5000FULL);
+
+	if (autoneg == AUTONEG_ENABLE) {
+		/* N-way force */
+		auto_nego = rtl_mdio_read(hw, MII_ADVERTISE);
+		auto_nego &= ~(ADVERTISE_10HALF | ADVERTISE_10FULL |
+		               ADVERTISE_100HALF | ADVERTISE_100FULL |
+		               ADVERTISE_PAUSE_CAP | ADVERTISE_PAUSE_ASYM);
+
+		if (adv & ADVERTISE_10_HALF)
+			auto_nego |= ADVERTISE_10HALF;
+		if (adv & ADVERTISE_10_FULL)
+			auto_nego |= ADVERTISE_10FULL;
+		if (adv & ADVERTISE_100_HALF)
+			auto_nego |= ADVERTISE_100HALF;
+		if (adv & ADVERTISE_100_FULL)
+			auto_nego |= ADVERTISE_100FULL;
+		if (adv & ADVERTISE_1000_HALF)
+			giga_ctrl |= ADVERTISE_1000HALF;
+		if (adv & ADVERTISE_1000_FULL)
+			giga_ctrl |= ADVERTISE_1000FULL;
+		if (adv & ADVERTISE_2500_FULL)
+			ctrl_2500 |= RTK_ADVERTISE_2500FULL;
+		if (adv & ADVERTISE_5000_FULL)
+			ctrl_2500 |= RTK_ADVERTISE_5000FULL;
+
+		/* Flow control */
+		if (hw->fcpause == rtl_fc_full)
+			auto_nego |= ADVERTISE_PAUSE_CAP | ADVERTISE_PAUSE_ASYM;
+
+		rtl_mdio_write(hw, 0x1f, 0x0000);
+		rtl_mdio_write(hw, MII_ADVERTISE, auto_nego);
+		rtl_mdio_write(hw, MII_CTRL1000, giga_ctrl);
+		rtl_mdio_direct_write_phy_ocp(hw, 0xA5D4, ctrl_2500);
+		rtl_phy_restart_nway(hw);
+		mdelay(20);
+	} else {
+		/* True force */
+		if (speed == SPEED_10 || speed == SPEED_100)
+			rtl_phy_setup_force_mode(hw, speed, duplex);
+		else
+			goto out;
+	}
+	hw->autoneg = autoneg;
+	hw->speed = speed;
+	hw->duplex = duplex;
+	hw->advertising = adv;
+
+	rc = 0;
+out:
+	return rc;
+}
+
+int
+rtl_set_speed(struct rtl_hw *hw)
+{
+	int ret;
+
+	ret = rtl_set_speed_xmii(hw, hw->autoneg, hw->speed, hw->duplex,
+	                         hw->advertising);
+
+	return ret;
+}
+
diff --git a/drivers/net/r8169/r8169_phy.h b/drivers/net/r8169/r8169_phy.h
index 599a8957c4..7675fd6cc9 100644
--- a/drivers/net/r8169/r8169_phy.h
+++ b/drivers/net/r8169/r8169_phy.h
@@ -141,5 +141,8 @@ void rtl_powerdown_pll(struct rtl_hw *hw);
 
 void rtl_hw_ephy_config(struct rtl_hw *hw);
 void rtl_hw_phy_config(struct rtl_hw *hw);
+
+int rtl_set_speed(struct rtl_hw *hw);
+
 #endif
 
-- 
2.34.1