net: ethernet: mtk_eth_soc: support ethernet passive mux

In order to support SFP/PHY dynamic switching, we add a passive mux to
the SerDes path that connects with the SFP cage and PHY. We then use
the GPIO to control this mux, which will switch to the SFP framework
when the SFP module is inserted and will switch to the PHY framework
when the SFP module is removed.
Signed-off-by: Bo-Cun Chen <bc-bocun.chen@mediatek.com>

Author: Bo-Cun Chen

drivers/net/ethernet/mediatek/mtk_eth_soc.c

@@ -24,6 +24,7 @@
 #include <linux/pcs/pcs-mtk-lynxi.h>
 #include <linux/pcs/pcs.h>
 #include <linux/phy/phy.h>
+#include <linux/gpio/consumer.h>
 #include <linux/jhash.h>
 #include <linux/bitfield.h>
 #include <net/dsa.h>
@@ -4918,6 +4919,207 @@ static const struct net_device_ops mtk_netdev_ops = {
 	.ndo_select_queue	= mtk_select_queue,
 };
 
+static void mux_poll(struct work_struct *work)
+{
+	struct mtk_mux *mux = container_of(work, struct mtk_mux, poll.work);
+	struct mtk_mac *mac = mux->mac;
+	struct mtk_eth *eth = mac->hw;
+	struct net_device *dev = eth->netdev[mac->id];
+	unsigned int new_channel;
+	int sfp_present;
+
+	if (IS_ERR(mux->mod_def0_gpio) || IS_ERR(mux->chan_sel_gpio))
+		goto reschedule;
+
+	sfp_present = gpiod_get_value_cansleep(mux->mod_def0_gpio);
+	new_channel = sfp_present ? mux->sfp_present_channel : !mux->sfp_present_channel;
+
+	if (mux->channel == new_channel || !netif_running(dev))
+		goto reschedule;
+
+	rtnl_lock();
+
+	mtk_stop(dev);
+
+	mac->of_node = mux->data[new_channel]->of_node;
+	mac->phylink = mux->data[new_channel]->phylink;
+
+	dev_info(eth->dev, "ethernet mux: switch to channel%d\n", new_channel);
+
+	gpiod_set_value_cansleep(mux->chan_sel_gpio, new_channel);
+
+	mtk_open(dev);
+
+	rtnl_unlock();
+
+	mux->channel = new_channel;
+
+reschedule:
+	mod_delayed_work(system_wq, &mux->poll, msecs_to_jiffies(100));
+}
+
+static int mtk_add_mux_channel(struct mtk_mux *mux, struct device_node *np)
+{
+	const __be32 *_id = of_get_property(np, "reg", NULL);
+	struct mtk_mac *mac = mux->mac;
+	struct mtk_eth *eth = mac->hw;
+	struct mtk_mux_data *data;
+	struct phylink *phylink;
+	phy_interface_t phy_mode;
+	int id, err;
+
+	if (!_id) {
+		dev_err(eth->dev, "missing mux channel id\n");
+		return -EINVAL;
+	}
+
+	id = be32_to_cpup(_id);
+	if (id < 0 || id > 1) {
+		dev_err(eth->dev, "%d is not a valid mux channel id\n", id);
+		return -EINVAL;
+	}
+
+	data = kmalloc(sizeof(*data), GFP_KERNEL);
+	if (unlikely(!data)) {
+		dev_err(eth->dev, "failed to create mux data structure\n");
+		return -ENOMEM;
+	}
+
+	err = of_get_phy_mode(np, &phy_mode);
+	if (err) {
+		dev_err(eth->dev, "incorrect phy-mode\n");
+		goto err_free_data;
+	}
+
+	phylink = phylink_create(&mux->mac->phylink_config,
+				 of_fwnode_handle(np),
+				 phy_mode, &mtk_phylink_ops);
+	if (IS_ERR(phylink)) {
+		dev_err(eth->dev, "failed to create phylink structure\n");
+		err = PTR_ERR(phylink);
+		goto err_free_data;
+	}
+
+	data->of_node = np;
+	data->phylink = phylink;
+	mux->data[id] = data;
+
+	return 0;
+
+err_free_data:
+	kfree(data);
+	return err;
+}
+
+static void mtk_release_mux(struct mtk_eth *eth, int id)
+{
+	struct mtk_mux *mux = eth->mux[id];
+	int i;
+
+	if (!mux)
+		return;
+
+	cancel_delayed_work_sync(&mux->poll);
+
+	if (!IS_ERR_OR_NULL(mux->mod_def0_gpio))
+		gpiod_put(mux->mod_def0_gpio);
+
+	if (!IS_ERR_OR_NULL(mux->chan_sel_gpio))
+		gpiod_put(mux->chan_sel_gpio);
+
+	for (i = 0; i < 2; i++) {
+		if (mux->data[i]) {
+			if (mux->data[i]->phylink)
+				phylink_destroy(mux->data[i]->phylink);
+			kfree(mux->data[i]);
+		}
+	}
+	kfree(mux);
+	eth->mux[id] = NULL;
+}
+
+static void mtk_release_all_muxes(struct mtk_eth *eth)
+{
+	int i;
+	for (i = 0; i < MTK_MAX_DEVS; i++)
+		mtk_release_mux(eth, i);
+}
+
+static int mtk_add_mux(struct mtk_eth *eth, struct device_node *np)
+{
+	const __be32 *_id = of_get_property(np, "reg", NULL);
+	struct device_node *child;
+	struct mtk_mux *mux;
+	unsigned int id;
+	int err;
+
+	if (!_id) {
+		dev_err(eth->dev, "missing attach mac id\n");
+		return -EINVAL;
+	}
+
+	id = be32_to_cpup(_id);
+	if (id < 0 || id >= MTK_MAX_DEVS) {
+		dev_err(eth->dev, "%d is not a valid attach mac id\n", id);
+		return -EINVAL;
+	}
+
+	mux = kmalloc(sizeof(struct mtk_mux), GFP_KERNEL);
+	if (unlikely(!mux)) {
+		dev_err(eth->dev, "failed to create mux structure\n");
+		return -ENOMEM;
+	}
+
+	eth->mux[id] = mux;
+	mux->mac = eth->mac[id];
+	mux->channel = 0;
+
+	mux->mod_def0_gpio = fwnode_gpiod_get_index(of_fwnode_handle(np),
+				"mod-def0", 0, GPIOD_IN |
+				GPIOD_FLAGS_BIT_NONEXCLUSIVE, "?");
+
+	if (IS_ERR(mux->mod_def0_gpio)) {
+		dev_err(eth->dev, "failed to requset gpio for mod-def0\n");
+		err = PTR_ERR(mux->mod_def0_gpio);
+		goto err_free_mux;
+	}
+
+	mux->chan_sel_gpio = fwnode_gpiod_get_index(of_fwnode_handle(np),
+				"chan-sel", 0, GPIOD_OUT_LOW, "?");
+
+	if (IS_ERR(mux->chan_sel_gpio)) {
+		dev_err(eth->dev, "failed to requset gpio for chan-sel\n");
+		err = PTR_ERR(mux->chan_sel_gpio);
+		goto err_put_mod_def0;
+	}
+
+	of_property_read_u32(np, "sfp-present-channel",
+		&mux->sfp_present_channel);
+
+	for_each_child_of_node(np, child) {
+		err = mtk_add_mux_channel(mux, child);
+		if (err) {
+			dev_err(eth->dev, "failed to add mtk_mux\n");
+			of_node_put(child);
+			goto err_put_chan_sel;
+		}
+	}
+
+	INIT_DELAYED_WORK(&mux->poll, mux_poll);
+	mod_delayed_work(system_wq, &mux->poll, msecs_to_jiffies(3000));
+
+	return 0;
+
+err_put_chan_sel:
+	gpiod_put(mux->chan_sel_gpio);
+err_put_mod_def0:
+	gpiod_put(mux->mod_def0_gpio);
+err_free_mux:
+	kfree(mux);
+	eth->mux[id] = NULL;
+	return err;
+}
+
 static int mtk_add_mac(struct mtk_eth *eth, struct device_node *np)
 {
 	const __be32 *_id = of_get_property(np, "reg", NULL);
@@ -5248,7 +5450,7 @@ static int mtk_setup_legacy_sram(struct mtk_eth *eth, struct resource *res)
 static int mtk_probe(struct platform_device *pdev)
 {
 	struct resource *res = NULL;
-	struct device_node *mac_np;
+	struct device_node *mac_np, *mux_np;
 	struct mtk_eth *eth;
 	int err, i;
 
@@ -5453,6 +5655,26 @@ static int mtk_probe(struct platform_device *pdev)
 			goto err_free_dev;
 	}
 
+	mux_np = of_get_child_by_name(eth->dev->of_node, "mux-bus");
+	if (mux_np) {
+		struct device_node *child;
+
+		for_each_available_child_of_node(mux_np, child) {
+			if (!of_device_is_compatible(child,
+						     "mediatek,eth-mux"))
+				continue;
+
+			if (!of_device_is_available(child))
+				continue;
+
+			err = mtk_add_mux(eth, child);
+			if (err)
+				dev_err(&pdev->dev, "failed to add mux\n");
+
+			of_node_put(mux_np);
+		};
+	}
+
 	if (eth->soc->offload_version) {
 		u8 ppe_num = eth->soc->ppe_num;
 
@@ -5510,6 +5732,7 @@ static int mtk_probe(struct platform_device *pdev)
 	mtk_unreg_dev(eth);
 err_deinit_ppe:
 	mtk_ppe_deinit(eth);
+	mtk_release_all_muxes(eth);
 	mtk_mdio_cleanup(eth);
 err_free_dev:
 	mtk_free_dev(eth);
@@ -5546,6 +5769,7 @@ static void mtk_remove(struct platform_device *pdev)
 	mtk_cleanup(eth);
 	free_netdev(eth->dummy_dev);
 	mtk_mdio_cleanup(eth);
+	mtk_release_all_muxes(eth);
 }
 
 static const struct mtk_soc_data mt2701_data = {

drivers/net/ethernet/mediatek/mtk_eth_soc.h

@@ -725,6 +725,7 @@ struct mtk_tx_dma_v2 {
 
 struct mtk_eth;
 struct mtk_mac;
+struct mtk_mux;
 
 struct mtk_xdp_stats {
 	u64 rx_xdp_redirect;
@@ -1390,6 +1391,7 @@ struct mtk_eth {
 	struct net_device		*dummy_dev;
 	struct net_device		*netdev[MTK_MAX_DEVS];
 	struct mtk_mac			*mac[MTK_MAX_DEVS];
+	struct mtk_mux			*mux[MTK_MAX_DEVS];
 	int				irq[MTK_FE_IRQ_NUM];
 	u32				msg_enable;
 	unsigned long			sysclk;
@@ -1476,6 +1478,27 @@ struct mtk_mac {
 	struct notifier_block		device_notifier;
 };
 
+/* struct mtk_mux_data -	the structure that holds the private data about the
+ *			 Passive MUXs of the SoC
+ */
+struct mtk_mux_data {
+	struct device_node		*of_node;
+	struct phylink			*phylink;
+};
+
+/* struct mtk_mux -	the structure that holds the info about the Passive MUXs of the
+ *			SoC
+ */
+struct mtk_mux {
+	struct delayed_work		poll;
+	struct gpio_desc		*mod_def0_gpio;
+	struct gpio_desc		*chan_sel_gpio;
+	struct mtk_mux_data		*data[2];
+	struct mtk_mac			*mac;
+	unsigned int			channel;
+	unsigned int			sfp_present_channel;
+};
+
 /* the struct describing the SoC. these are declared in the soc_xyz.c files */
 extern const struct of_device_id of_mtk_match[];