[v7,10/17] net/r8169: add link status and interrupt management

Message ID 20241112095804.42091-11-howard_wang@realsil.com.cn (mailing list archive)
State Changes Requested, archived
Delegated to: Ferruh Yigit
Headers
Series modify code as suggested by the maintainer |

Checks

Context Check Description
ci/checkpatch warning coding style issues

Commit Message

Howard Wang Nov. 12, 2024, 9:57 a.m. UTC
Signed-off-by: Howard Wang <howard_wang@realsil.com.cn>
---
 doc/guides/nics/features/r8169.ini |   5 +
 drivers/net/r8169/r8169_compat.h   |   1 +
 drivers/net/r8169/r8169_ethdev.c   | 278 ++++++++++++++++++++++++++++-
 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 +
 8 files changed, 417 insertions(+), 5 deletions(-)
  

Patch

diff --git a/doc/guides/nics/features/r8169.ini b/doc/guides/nics/features/r8169.ini
index dd1ce4db5c..ccb4cd9c23 100644
--- a/doc/guides/nics/features/r8169.ini
+++ b/doc/guides/nics/features/r8169.ini
@@ -4,6 +4,11 @@ 
 ; Refer to default.ini for the full list of available PMD features.
 ;
 [Features]
+Speed capabilities   = Y
+Link speed configuration = Y
+Link status          = Y
+Link status event    = Y
+Flow control         = Y
 Linux                = Y
 x86-32               = Y
 x86-64               = Y
diff --git a/drivers/net/r8169/r8169_compat.h b/drivers/net/r8169/r8169_compat.h
index fa23f11843..7b2cd08651 100644
--- a/drivers/net/r8169/r8169_compat.h
+++ b/drivers/net/r8169/r8169_compat.h
@@ -374,6 +374,7 @@  enum RTL_register_content {
 
 	/* PHY status */
 	PowerSaveStatus = 0x80,
+	_5000bpsF       = 0x1000,
 	_2500bpsF       = 0x400,
 	TxFlowCtrl      = 0x40,
 	RxFlowCtrl      = 0x20,
diff --git a/drivers/net/r8169/r8169_ethdev.c b/drivers/net/r8169/r8169_ethdev.c
index 2f845ce1e6..a946fcec3c 100644
--- a/drivers/net/r8169/r8169_ethdev.c
+++ b/drivers/net/r8169/r8169_ethdev.c
@@ -30,6 +30,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);
+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
@@ -48,6 +51,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
@@ -56,6 +63,118 @@  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)) {
+			rte_delay_ms(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.
@@ -65,8 +184,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);
@@ -85,6 +209,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);
 
 	return 0;
@@ -100,6 +232,9 @@  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;
+
+	rtl_disable_intr(hw);
 
 	rtl_nic_reset(hw);
 
@@ -112,18 +247,137 @@  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);
+
+	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;
@@ -133,6 +387,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;
 }
 
@@ -140,6 +408,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;
@@ -160,6 +429,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 */
@@ -186,6 +457,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 e1f5489973..3434a5a1b3 100644
--- a/drivers/net/r8169/r8169_ethdev.h
+++ b/drivers/net/r8169/r8169_ethdev.h
@@ -99,6 +99,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 4db2f0040e..2a4f58732f 100644
--- a/drivers/net/r8169/r8169_hw.c
+++ b/drivers/net/r8169/r8169_hw.c
@@ -958,7 +958,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)
 {
@@ -1088,13 +1088,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 9519739740..b243ca587d 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 f5cd192d13..85fdc9b648 100644
--- a/drivers/net/r8169/r8169_phy.c
+++ b/drivers/net/r8169/r8169_phy.c
@@ -776,3 +776,124 @@  rtl_hw_phy_config(struct rtl_hw *hw)
 	if (HW_HAS_WRITE_PHY_MCU_RAM_CODE(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);
+		rte_delay_ms(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 2a8f2bb7f8..1ed90d7cda 100644
--- a/drivers/net/r8169/r8169_phy.h
+++ b/drivers/net/r8169/r8169_phy.h
@@ -141,4 +141,7 @@  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