[PATCH net-next 3/4] net: phy: Automatically set-up cable test netdev_ops

[Florian Fainelli <f.fainelli@gmail.com>]

Upon attach, override the net_device operations with the PHY library
cable test operations and conversely, upon detach, revert to the
original net_device operations.

This will allows us in a subsequent patch to finally decouple the
ethtool/cabletest from the PHY library hard depenencies.

Signed-off-by: Florian Fainelli <f.fainelli@gmail.com>
---
 drivers/net/phy/phy_device.c | 32 ++++++++++++++++++++++++++++++++
 include/linux/phy.h          |  2 ++
 2 files changed, 34 insertions(+)

diff --git a/drivers/net/phy/phy_device.c b/drivers/net/phy/phy_device.c
index eb1068a77ce1..100d85541a06 100644
--- a/drivers/net/phy/phy_device.c
+++ b/drivers/net/phy/phy_device.c
@@ -1246,6 +1246,36 @@ phy_standalone_show(struct device *dev, struct device_attribute *attr,
 }
 static DEVICE_ATTR_RO(phy_standalone);
 
+static int phy_setup_netdev_ops(struct net_device *dev)
+{
+	struct phy_device *phydev = dev->phydev;
+	struct net_device_ops *ops;
+
+	ops = devm_kzalloc(&phydev->mdio.dev, sizeof(*ops), GFP_KERNEL);
+	if (!ops)
+		return -ENOMEM;
+
+	phydev->orig_ndo_ops = dev->netdev_ops;
+	if (phydev->orig_ndo_ops)
+		memcpy(ops, phydev->orig_ndo_ops, sizeof(*ops));
+
+	ops->ndo_cable_test_start = phy_start_cable_test;
+	ops->ndo_cable_test_tdr_start = phy_start_cable_test_tdr;
+
+	dev->netdev_ops = ops;
+
+	return 0;
+}
+
+static void phy_teardown_netdev_ops(struct net_device *dev)
+{
+	struct phy_device *phydev = dev->phydev;
+
+	if (phydev->orig_ndo_ops)
+		dev->netdev_ops = phydev->orig_ndo_ops;
+	phydev->orig_ndo_ops = NULL;
+}
+
 /**
  * phy_sfp_attach - attach the SFP bus to the PHY upstream network device
  * @upstream: pointer to the phy device
@@ -1380,6 +1410,7 @@ int phy_attach_direct(struct net_device *dev, struct phy_device *phydev,
 	if (dev) {
 		phydev->attached_dev = dev;
 		dev->phydev = phydev;
+		phy_setup_netdev_ops(dev);
 
 		if (phydev->sfp_bus_attached)
 			dev->sfp_bus = phydev->sfp_bus;
@@ -1676,6 +1707,7 @@ void phy_detach(struct phy_device *phydev)
 
 	phy_suspend(phydev);
 	if (dev) {
+		phy_teardown_netdev_ops(dev);
 		phydev->attached_dev->phydev = NULL;
 		phydev->attached_dev = NULL;
 	}
diff --git a/include/linux/phy.h b/include/linux/phy.h
index 53b95c52869d..04e35afa43ae 100644
--- a/include/linux/phy.h
+++ b/include/linux/phy.h
@@ -544,6 +544,8 @@ struct phy_device {
 	/* MACsec management functions */
 	const struct macsec_ops *macsec_ops;
 #endif
+	/* Original attached network device netdev_ops pointer */
+	const struct net_device_ops *orig_ndo_ops;
 };
 #define to_phy_device(d) container_of(to_mdio_device(d), \
 				      struct phy_device, mdio)
-- 
2.25.1