net: ethernet: mediatek: mtkmux: harden mux/phylink lifecycle

Author: rmandrad

drivers/net/ethernet/mediatek/mtk_eth_soc.c

@@ -5360,65 +5360,77 @@ static void mux_poll(struct work_struct *work)
 	struct mtk_eth *eth = mac->hw;
 	struct net_device *dev = eth->netdev[mac->id];
 	unsigned int new_channel;
-	struct phylink *tmp_pl;
+	struct phylink *old_pl;
 	int sfp_present;
+	int err;
+	bool running;
+	bool transitional_old_pl;
 
 	//dev_info(eth->dev, "ethernet mux: %s:%d\n",__func__,__LINE__);
 	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);
+	if (sfp_present < 0)
+		goto reschedule;
 	new_channel = sfp_present ? mux->sfp_present_channel : !mux->sfp_present_channel;
 
-	if (mux->channel == new_channel || !netif_running(dev))
+	if (mux->channel == new_channel)
 		goto reschedule;
 
+	running = netif_running(dev);
+
 	dev_info(eth->dev, "ethernet mux: line:%d new channel:%d,sfp:%d\n",__LINE__, new_channel,sfp_present);
 
+	if (mux->data[mux->channel] && mux->data[mux->channel]->phylink)
+		old_pl = mux->data[mux->channel]->phylink;
+	else
+		old_pl = mac->phylink;
+	transitional_old_pl = old_pl && old_pl == mux->initial_phylink;
+
 	rtnl_lock();
-	mtk_stop(dev);
+	if (running)
+		mtk_stop(dev);
 	rtnl_unlock();
 
-	/* Destroy old phylink if it exists */
-	if (mux->data[mux->channel] && mux->data[mux->channel]->phylink) {
-		tmp_pl = mux->data[mux->channel]->phylink;
+	if (old_pl) {
 		dev_info(eth->dev, "Destroying phylink for channel %u\n", mux->channel);
-	} else {
-		/* phylink was created by mtk_add_mac,
-		   we need to release the reference to available PCS from phylink config
-		*/
-		tmp_pl = mac->phylink;
+		if (mux->data[mux->channel] &&
+		    mux->data[mux->channel]->phylink == old_pl)
+			mux->data[mux->channel]->phylink = NULL;
+		phylink_destroy(old_pl);
+		if (transitional_old_pl)
+			mux->initial_phylink = NULL;
 	}
-	if (tmp_pl) {
-		phylink_destroy(tmp_pl);
-		mux->data[mux->channel]->phylink = NULL;
-	}
-
-	dev_info(eth->dev, "ethernet mux: switch to channel%d\n", new_channel);
 
-	/* Create new phylink if not yet present */
+	/* Build target phylink after tearing down the old one, since both
+	 * instances share the same PCS objects in mac->available_pcs.
+	 */
 	if (!mux->data[new_channel]->phylink) {
 		mux->data[new_channel]->phylink = mtk_mux_create_phylink(mux, new_channel);
 		if (IS_ERR(mux->data[new_channel]->phylink)) {
 			dev_err(eth->dev, "Failed to create new phylink\n");
-			mux->data[new_channel]->phylink=NULL;
-			goto out_unlock;
+			mux->data[new_channel]->phylink = NULL;
+			goto reschedule;
 		}
 	}
 
+	rtnl_lock();
+	dev_info(eth->dev, "ethernet mux: switch to channel%d\n", new_channel);
 	mac->of_node = mux->data[new_channel]->of_node;
 	mac->phylink = mux->data[new_channel]->phylink;
 
-	rtnl_lock();
-	mtk_open(dev);
-	rtnl_unlock();
+	if (running) {
+		err = mtk_open(dev);
+		if (err)
+			dev_err(eth->dev, "ethernet mux: failed to open dev on channel %u: %d\n",
+				new_channel, err);
+	}
 
 	gpiod_set_value_cansleep(mux->chan_sel_gpio, new_channel);
 	mux->channel = new_channel;
-	goto reschedule;
-
-out_unlock:
 	rtnl_unlock();
+
 reschedule:
 	mod_delayed_work(system_wq, &mux->poll, msecs_to_jiffies(100));
 }
@@ -5499,6 +5511,13 @@ static void mtk_release_mux(struct mtk_eth *eth, int id)
 			kfree(mux->data[i]);
 		}
 	}
+
+	/* Destroy transitional phylink created by mtk_add_mac() when it was
+	 * never promoted into a mux channel instance.
+	 */
+	if (mux->initial_phylink)
+		phylink_destroy(mux->initial_phylink);
+
 	kfree(mux);
 	eth->mux[id] = NULL;
 }
@@ -5516,7 +5535,8 @@ static int mtk_add_mux(struct mtk_eth *eth, struct device_node *np)
 	struct device_node *child;
 	struct mtk_mux *mux;
 	unsigned int id;
-	int err;
+	unsigned int initial_channel;
+	int err, sfp_present;
 
 	if (!_id) {
 		dev_err(eth->dev, "missing attach mac id\n");
@@ -5529,15 +5549,15 @@ static int mtk_add_mux(struct mtk_eth *eth, struct device_node *np)
 		return -EINVAL;
 	}
 
-	mux = kmalloc(sizeof(struct mtk_mux), GFP_KERNEL);
+	mux = kzalloc(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;//more than channels, just to make current channel invalid for switching the first time the gpio is read
+	mux->initial_phylink = mux->mac ? mux->mac->phylink : NULL;
 
 	mux->mod_def0_gpio = fwnode_gpiod_get_index(of_fwnode_handle(np),
 				"mod-def0", 0, GPIOD_IN |
@@ -5558,8 +5578,11 @@ static int mtk_add_mux(struct mtk_eth *eth, struct device_node *np)
 		goto err_put_mod_def0;
 	}
 
-	of_property_read_u32(np, "sfp-present-channel",
-		&mux->sfp_present_channel);
+	if (of_property_read_u32(np, "sfp-present-channel",
+				 &mux->sfp_present_channel))
+		mux->sfp_present_channel = 0;
+	else if (mux->sfp_present_channel > 1)
+		mux->sfp_present_channel = 1;
 
 	for_each_child_of_node(np, child) {
 		err = mtk_add_mux_channel(mux, child);
@@ -5571,7 +5594,18 @@ static int mtk_add_mux(struct mtk_eth *eth, struct device_node *np)
 		//should set initial mux->channel be set if ! mux->sfp_present_channel?
 	}
 
-	gpiod_set_value_cansleep(mux->chan_sel_gpio, mux->sfp_present_channel ? 0 : 1);
+	/* Configure initial mux channel based on current module presence. */
+	sfp_present = gpiod_get_value_cansleep(mux->mod_def0_gpio);
+	if (sfp_present < 0) {
+		dev_warn(eth->dev, "failed to read mod-def0 gpio, defaulting to non-SFP channel\n");
+		initial_channel = !mux->sfp_present_channel;
+	} else {
+		initial_channel = sfp_present ? mux->sfp_present_channel :
+						!mux->sfp_present_channel;
+	}
+	gpiod_set_value_cansleep(mux->chan_sel_gpio, initial_channel);
+	/* Force first poll pass to (re)build phylink for selected channel. */
+	mux->channel = !initial_channel;
 
 	dev_info(eth->dev, "ethernet mux: line:%d added new mux\n",__LINE__);
 	INIT_DELAYED_WORK(&mux->poll, mux_poll);

drivers/net/ethernet/mediatek/mtk_eth_soc.h

@@ -1574,6 +1574,7 @@ struct mtk_mux {
 	struct gpio_desc		*chan_sel_gpio;
 	struct mtk_mux_data		*data[2];
 	struct mtk_mac			*mac;
+	struct phylink			*initial_phylink;
 	unsigned int			channel;
 	unsigned int			sfp_present_channel;
 };

drivers/net/pcs/pcs.c

@@ -30,6 +30,12 @@ int register_fwnode_pcs_notifier(struct notifier_block *nb)
 }
 EXPORT_SYMBOL_GPL(register_fwnode_pcs_notifier);
 
+int unregister_fwnode_pcs_notifier(struct notifier_block *nb)
+{
+	return blocking_notifier_chain_unregister(&fwnode_pcs_notify_list, nb);
+}
+EXPORT_SYMBOL_GPL(unregister_fwnode_pcs_notifier);
+
 struct phylink_pcs *fwnode_pcs_simple_get(struct fwnode_reference_args *pcsspec,
 					  void *data)
 {

drivers/net/phy/phylink.c

@@ -63,6 +63,7 @@ struct phylink {
 	/* List of available PCS */
 	struct list_head pcs_list;
 	struct notifier_block fwnode_pcs_nb;
+	bool fwnode_pcs_nb_registered;
 
 	/* What interface are supported by the current link.
 	 * Can change on removal or addition of new PCS.
@@ -2061,7 +2062,12 @@ struct phylink *phylink_create(struct phylink_config *config,
 
 	if (!phy_interface_empty(config->pcs_interfaces)) {
 		pl->fwnode_pcs_nb.notifier_call = pcs_provider_notify;
-		register_fwnode_pcs_notifier(&pl->fwnode_pcs_nb);
+		ret = register_fwnode_pcs_notifier(&pl->fwnode_pcs_nb);
+		if (ret && ret != -EOPNOTSUPP) {
+			kfree(pl);
+			return ERR_PTR(ret);
+		}
+		pl->fwnode_pcs_nb_registered = !ret;
 	}
 
 	pl->config = config;
@@ -2071,8 +2077,8 @@ struct phylink *phylink_create(struct phylink_config *config,
 	} else if (config->type == PHYLINK_DEV) {
 		pl->dev = config->dev;
 	} else {
-		kfree(pl);
-		return ERR_PTR(-EINVAL);
+		ret = -EINVAL;
+		goto err_unregister_fwnode_pcs_notifier;
 	}
 
 	pl->mac_supports_eee_ops = phylink_mac_implements_lpi(mac_ops);
@@ -2105,28 +2111,28 @@ struct phylink *phylink_create(struct phylink_config *config,
 	phylink_validate(pl, pl->supported, &pl->link_config);
 
 	ret = phylink_parse_mode(pl, fwnode);
-	if (ret < 0) {
-		kfree(pl);
-		return ERR_PTR(ret);
-	}
+	if (ret < 0)
+		goto err_unregister_fwnode_pcs_notifier;
 
 	if (pl->cfg_link_an_mode == MLO_AN_FIXED) {
 		ret = phylink_parse_fixedlink(pl, fwnode);
-		if (ret < 0) {
-			kfree(pl);
-			return ERR_PTR(ret);
-		}
+		if (ret < 0)
+			goto err_unregister_fwnode_pcs_notifier;
 	}
 
 	pl->req_link_an_mode = pl->cfg_link_an_mode;
 
 	ret = phylink_register_sfp(pl, fwnode);
-	if (ret < 0) {
-		kfree(pl);
-		return ERR_PTR(ret);
-	}
+	if (ret < 0)
+		goto err_unregister_fwnode_pcs_notifier;
 
 	return pl;
+
+err_unregister_fwnode_pcs_notifier:
+	if (pl->fwnode_pcs_nb_registered)
+		unregister_fwnode_pcs_notifier(&pl->fwnode_pcs_nb);
+	kfree(pl);
+	return ERR_PTR(ret);
 }
 EXPORT_SYMBOL_GPL(phylink_create);
 
@@ -2151,6 +2157,9 @@ void phylink_destroy(struct phylink *pl)
 	list_for_each_entry_safe(pcs, tmp, &pl->pcs_list, list)
 		list_del(&pcs->list);
 
+	if (pl->fwnode_pcs_nb_registered)
+		unregister_fwnode_pcs_notifier(&pl->fwnode_pcs_nb);
+
 	cancel_work_sync(&pl->resolve);
 	kfree(pl);
 }

include/linux/pcs/pcs.h

@@ -22,6 +22,18 @@ enum fwnode_pcs_notify_event {
  */
 int register_fwnode_pcs_notifier(struct notifier_block *nb);
 
+/**
+ * unregister_fwnode_pcs_notifier - Unregister a notifier block for fwnode
+ *				    PCS events
+ * @nb: pointer to the notifier block
+ *
+ * Unregisters a notifier block previously registered with
+ * register_fwnode_pcs_notifier().
+ *
+ * Returns 0 or a negative error.
+ */
+int unregister_fwnode_pcs_notifier(struct notifier_block *nb);
+
 /**
  * fwnode_pcs_get - Retrieves a PCS from a firmware node
  * @fwnode: firmware node
@@ -80,6 +92,11 @@ static int register_fwnode_pcs_notifier(struct notifier_block *nb)
 	return -EOPNOTSUPP;
 }
 
+static int unregister_fwnode_pcs_notifier(struct notifier_block *nb)
+{
+	return -EOPNOTSUPP;
+}
+
 static inline struct phylink_pcs *fwnode_pcs_get(struct fwnode_handle *fwnode,
 						 int index)
 {