[PATCH net-next v3 5/5] net: lan743x: Add PCS/XPCS support for SFP on PCI11x1x

[Thangaraj Samynathan <thangaraj.s@microchip.com>]

Add a PCS MII bus and XPCS instance to support SFP modules on PCI11x1x
platforms.

Register a dedicated mdiobus for PCS access when SFP support is enabled
and initialize it with C45 read/write callbacks. Implement generic PCS
read/write functions that use the internal SGMII access functions for
the PCS.

Integrate the XPCS instance with phylink by providing a mac_select_pcs
callback. Support SGMII and 2.5GBASE-X interfaces in phylink, allowing
proper link configuration for SFP modules.

Cleanup the PCS mdiobus and XPCS instance during driver removal. Update
adapter structure to hold PCS mdiobus and XPCS references.

Signed-off-by: Thangaraj Samynathan <thangaraj.s@microchip.com>
---
 drivers/net/ethernet/microchip/lan743x_main.c | 86 ++++++++++++++++++-
 drivers/net/ethernet/microchip/lan743x_main.h |  3 +
 2 files changed, 86 insertions(+), 3 deletions(-)

diff --git a/drivers/net/ethernet/microchip/lan743x_main.c b/drivers/net/ethernet/microchip/lan743x_main.c
index 88a2d11552f8..485fbc678481 100644
--- a/drivers/net/ethernet/microchip/lan743x_main.c
+++ b/drivers/net/ethernet/microchip/lan743x_main.c
@@ -15,7 +15,6 @@
 #include <linux/rtnetlink.h>
 #include <linux/iopoll.h>
 #include <linux/crc16.h>
-#include <linux/phylink.h>
 
 #include "lan743x_main.h"
 #include "lan743x_ethtool.h"
@@ -1137,6 +1136,28 @@ static int lan743x_get_lsd(int speed, int duplex, u8 mss)
 	return lsd;
 }
 
+static int pci11x1x_pcs_read(struct mii_bus *bus, int addr, int devnum,
+			     int regnum)
+{
+	struct lan743x_adapter *adapter = bus->priv;
+
+	if (addr)
+		return -EOPNOTSUPP;
+
+	return lan743x_sgmii_read(adapter, devnum, regnum);
+}
+
+static int pci11x1x_pcs_write(struct mii_bus *bus, int addr, int devnum,
+			      int regnum, u16 val)
+{
+	struct lan743x_adapter *adapter = bus->priv;
+
+	if (addr)
+		return -EOPNOTSUPP;
+
+	return lan743x_sgmii_write(adapter, devnum, regnum, val);
+}
+
 static int lan743x_sgmii_mpll_set(struct lan743x_adapter *adapter,
 				  u16 baud)
 {
@@ -3199,6 +3220,18 @@ static void lan743x_mac_eee_enable(struct lan743x_adapter *adapter, bool enable)
 	lan743x_csr_write(adapter, MAC_CR, mac_cr);
 }
 
+static struct phylink_pcs *lan743x_phylink_mac_select(struct phylink_config *config,
+						      phy_interface_t interface)
+{
+	struct net_device *netdev = to_net_dev(config->dev);
+	struct lan743x_adapter *adapter = netdev_priv(netdev);
+
+	if (adapter->xpcs)
+		return adapter->xpcs;
+
+	return NULL;
+}
+
 static void lan743x_phylink_mac_config(struct phylink_config *config,
 				       unsigned int link_an_mode,
 				       const struct phylink_link_state *state)
@@ -3330,6 +3363,7 @@ static const struct phylink_mac_ops lan743x_phylink_mac_ops = {
 	.mac_link_up = lan743x_phylink_mac_link_up,
 	.mac_disable_tx_lpi = lan743x_mac_disable_tx_lpi,
 	.mac_enable_tx_lpi = lan743x_mac_enable_tx_lpi,
+	.mac_select_pcs = lan743x_phylink_mac_select,
 };
 
 static int lan743x_phylink_create(struct lan743x_adapter *adapter)
@@ -3353,6 +3387,7 @@ static int lan743x_phylink_create(struct lan743x_adapter *adapter)
 
 	switch (adapter->phy_interface) {
 	case PHY_INTERFACE_MODE_SGMII:
+	case PHY_INTERFACE_MODE_2500BASEX:
 		__set_bit(PHY_INTERFACE_MODE_SGMII,
 			  adapter->phylink_config.supported_interfaces);
 		__set_bit(PHY_INTERFACE_MODE_1000BASEX,
@@ -3416,12 +3451,13 @@ static int lan743x_phylink_connect(struct lan743x_adapter *adapter)
 	struct device_node *dn = adapter->pdev->dev.of_node;
 	struct net_device *dev = adapter->netdev;
 	struct phy_device *phydev;
-	int ret;
+	int ret = 0;
 
 	if (dn)
 		ret = phylink_of_phy_connect(adapter->phylink, dn, 0);
 
-	if (!dn || (ret && !lan743x_phy_handle_exists(dn))) {
+	if (!adapter->is_sfp_support_en &&
+	    (!dn || (ret && !lan743x_phy_handle_exists(dn)))) {
 		phydev = phy_find_first(adapter->mdiobus);
 		if (phydev) {
 			/* attach the mac to the phy */
@@ -3694,6 +3730,9 @@ static void lan743x_hardware_cleanup(struct lan743x_adapter *adapter)
 
 static void lan743x_mdiobus_cleanup(struct lan743x_adapter *adapter)
 {
+	if (adapter->xpcs)
+		xpcs_destroy_pcs(adapter->xpcs);
+
 	mdiobus_unregister(adapter->mdiobus);
 }
 
@@ -3806,6 +3845,42 @@ static int lan743x_hardware_init(struct lan743x_adapter *adapter,
 	return 0;
 }
 
+static int lan743x_pcs_mdiobus_init(struct lan743x_adapter *adapter)
+{
+	struct phylink_pcs *pcs;
+	int ret;
+
+	adapter->pcs_mdiobus = devm_mdiobus_alloc(&adapter->pdev->dev);
+	if (!(adapter->pcs_mdiobus)) {
+		ret = -ENOMEM;
+		goto return_error;
+	}
+
+	adapter->pcs_mdiobus->priv = (void *)adapter;
+	adapter->pcs_mdiobus->read_c45 = pci11x1x_pcs_read;
+	adapter->pcs_mdiobus->write_c45 = pci11x1x_pcs_write;
+	adapter->pcs_mdiobus->name = "lan743x-pcs-mdiobus-c45";
+	netif_dbg(adapter, drv, adapter->netdev, "lan743x-pcs-mdiobus-c45\n");
+	snprintf(adapter->pcs_mdiobus->id, MII_BUS_ID_SIZE, "pci-pcs-%s", pci_name(adapter->pdev));
+
+	if (!adapter->phy_interface)
+		lan743x_phy_interface_select(adapter);
+
+	pcs = xpcs_create_pcs_mdiodev(adapter->pcs_mdiobus, 0);
+	if (IS_ERR(pcs)) {
+		netdev_err(adapter->netdev, "failed to create xpcs\n");
+		ret = PTR_ERR(pcs);
+		goto return_error;
+	}
+
+	adapter->xpcs = pcs;
+	return 0;
+
+return_error:
+	mdiobus_free(adapter->pcs_mdiobus);
+	return ret;
+}
+
 static int lan743x_mdiobus_init(struct lan743x_adapter *adapter)
 {
 	int ret;
@@ -3927,6 +4002,11 @@ static int lan743x_pcidev_probe(struct pci_dev *pdev,
 	if (ret)
 		goto cleanup_hardware;
 
+	if (adapter->is_sfp_support_en) {
+		ret = lan743x_pcs_mdiobus_init(adapter);
+		if (ret)
+			goto cleanup_hardware;
+	}
 	adapter->netdev->netdev_ops = &lan743x_netdev_ops;
 	adapter->netdev->ethtool_ops = &lan743x_ethtool_ops;
 	adapter->netdev->features = NETIF_F_SG | NETIF_F_TSO |
diff --git a/drivers/net/ethernet/microchip/lan743x_main.h b/drivers/net/ethernet/microchip/lan743x_main.h
index fd1c2842b4c8..9740c2628f94 100644
--- a/drivers/net/ethernet/microchip/lan743x_main.h
+++ b/drivers/net/ethernet/microchip/lan743x_main.h
@@ -8,6 +8,7 @@
 #include <linux/gpio/machine.h>
 #include <linux/i2c.h>
 #include <linux/phy.h>
+#include <linux/pcs/pcs-xpcs.h>
 #include <linux/phylink.h>
 #include <linux/platform_device.h>
 #include <linux/property.h>
@@ -1132,6 +1133,7 @@ struct lan743x_sw_nodes {
 struct lan743x_adapter {
 	struct net_device       *netdev;
 	struct mii_bus		*mdiobus;
+	struct mii_bus		*pcs_mdiobus;
 	int                     msg_enable;
 #ifdef CONFIG_PM
 	u32			wolopts;
@@ -1169,6 +1171,7 @@ struct lan743x_adapter {
 	u32			flags;
 	u32			hw_cfg;
 	phy_interface_t		phy_interface;
+	struct phylink_pcs      *xpcs;
 	struct phylink		*phylink;
 	struct phylink_config	phylink_config;
 	int			rx_tstamp_filter;
-- 
2.34.1