kernel: 5.10: backport Marvell 88E1510/2 PHY SFP support

Backport upstream SFP support for the Marvell 88E1510/2 PHY-s.

Globalscale MOCHAbin uses this PHY for the hybrid
WAN port that has 1G SFP and 1G RJ45 with PoE PD
connected to it.

This allows the SFP port to be used on it as well as
parsing the SFP module details with ethtool.

Signed-off-by: Robert Marko <robert.marko@sartura.hr>

2 files changed, 710 insertions, 0 deletions

diff --git a/target/linux/generic/backport-5.10/742-v5.13-net-phy-marvell-refactor-HWMON-OOP-style.patch b/target/linux/generic/backport-5.10/742-v5.13-net-phy-marvell-refactor-HWMON-OOP-style.patch
new file mode 100644
index 0000000000..7bf59267fa
--- /dev/null
+++ b/target/linux/generic/backport-5.10/742-v5.13-net-phy-marvell-refactor-HWMON-OOP-style.patch
@@ -0,0 +1,549 @@
+From 41d26bf4aba070dfd2ab48866cc27a48ee6228c7 Mon Sep 17 00:00:00 2001
+From: =?UTF-8?q?Marek=20Beh=C3=BAn?= <kabel@kernel.org>
+Date: Tue, 20 Apr 2021 09:53:59 +0200
+Subject: [PATCH] net: phy: marvell: refactor HWMON OOP style
+MIME-Version: 1.0
+Content-Type: text/plain; charset=UTF-8
+Content-Transfer-Encoding: 8bit
+
+Use a structure of Marvell PHY specific HWMON methods to reduce code
+duplication. Store a pointer to this structure into the PHY driver's
+driver_data member.
+
+Signed-off-by: Marek Behún <kabel@kernel.org>
+Signed-off-by: David S. Miller <davem@davemloft.net>
+---
+ drivers/net/phy/marvell.c | 369 +++++++++++++-------------------------
+ 1 file changed, 125 insertions(+), 244 deletions(-)
+
+--- a/drivers/net/phy/marvell.c
++++ b/drivers/net/phy/marvell.c
+@@ -2134,6 +2134,19 @@ static int marvell_vct7_cable_test_get_s
+ }
+ 
+ #ifdef CONFIG_HWMON
++struct marvell_hwmon_ops {
++	int (*get_temp)(struct phy_device *phydev, long *temp);
++	int (*get_temp_critical)(struct phy_device *phydev, long *temp);
++	int (*set_temp_critical)(struct phy_device *phydev, long temp);
++	int (*get_temp_alarm)(struct phy_device *phydev, long *alarm);
++};
++
++static const struct marvell_hwmon_ops *
++to_marvell_hwmon_ops(const struct phy_device *phydev)
++{
++	return phydev->drv->driver_data;
++}
++
+ static int m88e1121_get_temp(struct phy_device *phydev, long *temp)
+ {
+ 	int oldpage;
+@@ -2177,75 +2190,6 @@ error:
+ 	return phy_restore_page(phydev, oldpage, ret);
+ }
+ 
+-static int m88e1121_hwmon_read(struct device *dev,
+-			       enum hwmon_sensor_types type,
+-			       u32 attr, int channel, long *temp)
+-{
+-	struct phy_device *phydev = dev_get_drvdata(dev);
+-	int err;
+-
+-	switch (attr) {
+-	case hwmon_temp_input:
+-		err = m88e1121_get_temp(phydev, temp);
+-		break;
+-	default:
+-		return -EOPNOTSUPP;
+-	}
+-
+-	return err;
+-}
+-
+-static umode_t m88e1121_hwmon_is_visible(const void *data,
+-					 enum hwmon_sensor_types type,
+-					 u32 attr, int channel)
+-{
+-	if (type != hwmon_temp)
+-		return 0;
+-
+-	switch (attr) {
+-	case hwmon_temp_input:
+-		return 0444;
+-	default:
+-		return 0;
+-	}
+-}
+-
+-static u32 m88e1121_hwmon_chip_config[] = {
+-	HWMON_C_REGISTER_TZ,
+-	0
+-};
+-
+-static const struct hwmon_channel_info m88e1121_hwmon_chip = {
+-	.type = hwmon_chip,
+-	.config = m88e1121_hwmon_chip_config,
+-};
+-
+-static u32 m88e1121_hwmon_temp_config[] = {
+-	HWMON_T_INPUT,
+-	0
+-};
+-
+-static const struct hwmon_channel_info m88e1121_hwmon_temp = {
+-	.type = hwmon_temp,
+-	.config = m88e1121_hwmon_temp_config,
+-};
+-
+-static const struct hwmon_channel_info *m88e1121_hwmon_info[] = {
+-	&m88e1121_hwmon_chip,
+-	&m88e1121_hwmon_temp,
+-	NULL
+-};
+-
+-static const struct hwmon_ops m88e1121_hwmon_hwmon_ops = {
+-	.is_visible = m88e1121_hwmon_is_visible,
+-	.read = m88e1121_hwmon_read,
+-};
+-
+-static const struct hwmon_chip_info m88e1121_hwmon_chip_info = {
+-	.ops = &m88e1121_hwmon_hwmon_ops,
+-	.info = m88e1121_hwmon_info,
+-};
+-
+ static int m88e1510_get_temp(struct phy_device *phydev, long *temp)
+ {
+ 	int ret;
+@@ -2308,92 +2252,6 @@ static int m88e1510_get_temp_alarm(struc
+ 	return 0;
+ }
+ 
+-static int m88e1510_hwmon_read(struct device *dev,
+-			       enum hwmon_sensor_types type,
+-			       u32 attr, int channel, long *temp)
+-{
+-	struct phy_device *phydev = dev_get_drvdata(dev);
+-	int err;
+-
+-	switch (attr) {
+-	case hwmon_temp_input:
+-		err = m88e1510_get_temp(phydev, temp);
+-		break;
+-	case hwmon_temp_crit:
+-		err = m88e1510_get_temp_critical(phydev, temp);
+-		break;
+-	case hwmon_temp_max_alarm:
+-		err = m88e1510_get_temp_alarm(phydev, temp);
+-		break;
+-	default:
+-		return -EOPNOTSUPP;
+-	}
+-
+-	return err;
+-}
+-
+-static int m88e1510_hwmon_write(struct device *dev,
+-				enum hwmon_sensor_types type,
+-				u32 attr, int channel, long temp)
+-{
+-	struct phy_device *phydev = dev_get_drvdata(dev);
+-	int err;
+-
+-	switch (attr) {
+-	case hwmon_temp_crit:
+-		err = m88e1510_set_temp_critical(phydev, temp);
+-		break;
+-	default:
+-		return -EOPNOTSUPP;
+-	}
+-	return err;
+-}
+-
+-static umode_t m88e1510_hwmon_is_visible(const void *data,
+-					 enum hwmon_sensor_types type,
+-					 u32 attr, int channel)
+-{
+-	if (type != hwmon_temp)
+-		return 0;
+-
+-	switch (attr) {
+-	case hwmon_temp_input:
+-	case hwmon_temp_max_alarm:
+-		return 0444;
+-	case hwmon_temp_crit:
+-		return 0644;
+-	default:
+-		return 0;
+-	}
+-}
+-
+-static u32 m88e1510_hwmon_temp_config[] = {
+-	HWMON_T_INPUT | HWMON_T_CRIT | HWMON_T_MAX_ALARM,
+-	0
+-};
+-
+-static const struct hwmon_channel_info m88e1510_hwmon_temp = {
+-	.type = hwmon_temp,
+-	.config = m88e1510_hwmon_temp_config,
+-};
+-
+-static const struct hwmon_channel_info *m88e1510_hwmon_info[] = {
+-	&m88e1121_hwmon_chip,
+-	&m88e1510_hwmon_temp,
+-	NULL
+-};
+-
+-static const struct hwmon_ops m88e1510_hwmon_hwmon_ops = {
+-	.is_visible = m88e1510_hwmon_is_visible,
+-	.read = m88e1510_hwmon_read,
+-	.write = m88e1510_hwmon_write,
+-};
+-
+-static const struct hwmon_chip_info m88e1510_hwmon_chip_info = {
+-	.ops = &m88e1510_hwmon_hwmon_ops,
+-	.info = m88e1510_hwmon_info,
+-};
+-
+ static int m88e6390_get_temp(struct phy_device *phydev, long *temp)
+ {
+ 	int sum = 0;
+@@ -2452,63 +2310,112 @@ error:
+ 	return ret;
+ }
+ 
+-static int m88e6390_hwmon_read(struct device *dev,
+-			       enum hwmon_sensor_types type,
+-			       u32 attr, int channel, long *temp)
++static int marvell_hwmon_read(struct device *dev, enum hwmon_sensor_types type,
++			      u32 attr, int channel, long *temp)
+ {
+ 	struct phy_device *phydev = dev_get_drvdata(dev);
+-	int err;
++	const struct marvell_hwmon_ops *ops = to_marvell_hwmon_ops(phydev);
++	int err = -EOPNOTSUPP;
+ 
+ 	switch (attr) {
+ 	case hwmon_temp_input:
+-		err = m88e6390_get_temp(phydev, temp);
++		if (ops->get_temp)
++			err = ops->get_temp(phydev, temp);
++		break;
++	case hwmon_temp_crit:
++		if (ops->get_temp_critical)
++			err = ops->get_temp_critical(phydev, temp);
++		break;
++	case hwmon_temp_max_alarm:
++		if (ops->get_temp_alarm)
++			err = ops->get_temp_alarm(phydev, temp);
++		break;
++	}
++
++	return err;
++}
++
++static int marvell_hwmon_write(struct device *dev, enum hwmon_sensor_types type,
++			       u32 attr, int channel, long temp)
++{
++	struct phy_device *phydev = dev_get_drvdata(dev);
++	const struct marvell_hwmon_ops *ops = to_marvell_hwmon_ops(phydev);
++	int err = -EOPNOTSUPP;
++
++	switch (attr) {
++	case hwmon_temp_crit:
++		if (ops->set_temp_critical)
++			err = ops->set_temp_critical(phydev, temp);
+ 		break;
+ 	default:
+-		return -EOPNOTSUPP;
++		fallthrough;
+ 	}
+ 
+ 	return err;
+ }
+ 
+-static umode_t m88e6390_hwmon_is_visible(const void *data,
+-					 enum hwmon_sensor_types type,
+-					 u32 attr, int channel)
++static umode_t marvell_hwmon_is_visible(const void *data,
++					enum hwmon_sensor_types type,
++					u32 attr, int channel)
+ {
++	const struct phy_device *phydev = data;
++	const struct marvell_hwmon_ops *ops = to_marvell_hwmon_ops(phydev);
++
+ 	if (type != hwmon_temp)
+ 		return 0;
+ 
+ 	switch (attr) {
+ 	case hwmon_temp_input:
+-		return 0444;
++		return ops->get_temp ? 0444 : 0;
++	case hwmon_temp_max_alarm:
++		return ops->get_temp_alarm ? 0444 : 0;
++	case hwmon_temp_crit:
++		return (ops->get_temp_critical ? 0444 : 0) |
++		       (ops->set_temp_critical ? 0200 : 0);
+ 	default:
+ 		return 0;
+ 	}
+ }
+ 
+-static u32 m88e6390_hwmon_temp_config[] = {
+-	HWMON_T_INPUT,
++static u32 marvell_hwmon_chip_config[] = {
++	HWMON_C_REGISTER_TZ,
+ 	0
+ };
+ 
+-static const struct hwmon_channel_info m88e6390_hwmon_temp = {
++static const struct hwmon_channel_info marvell_hwmon_chip = {
++	.type = hwmon_chip,
++	.config = marvell_hwmon_chip_config,
++};
++
++/* we can define HWMON_T_CRIT and HWMON_T_MAX_ALARM even though these are not
++ * defined for all PHYs, because the hwmon code checks whether the attributes
++ * exists via the .is_visible method
++ */
++static u32 marvell_hwmon_temp_config[] = {
++	HWMON_T_INPUT | HWMON_T_CRIT | HWMON_T_MAX_ALARM,
++	0
++};
++
++static const struct hwmon_channel_info marvell_hwmon_temp = {
+ 	.type = hwmon_temp,
+-	.config = m88e6390_hwmon_temp_config,
++	.config = marvell_hwmon_temp_config,
+ };
+ 
+-static const struct hwmon_channel_info *m88e6390_hwmon_info[] = {
+-	&m88e1121_hwmon_chip,
+-	&m88e6390_hwmon_temp,
++static const struct hwmon_channel_info *marvell_hwmon_info[] = {
++	&marvell_hwmon_chip,
++	&marvell_hwmon_temp,
+ 	NULL
+ };
+ 
+-static const struct hwmon_ops m88e6390_hwmon_hwmon_ops = {
+-	.is_visible = m88e6390_hwmon_is_visible,
+-	.read = m88e6390_hwmon_read,
++static const struct hwmon_ops marvell_hwmon_hwmon_ops = {
++	.is_visible = marvell_hwmon_is_visible,
++	.read = marvell_hwmon_read,
++	.write = marvell_hwmon_write,
+ };
+ 
+-static const struct hwmon_chip_info m88e6390_hwmon_chip_info = {
+-	.ops = &m88e6390_hwmon_hwmon_ops,
+-	.info = m88e6390_hwmon_info,
++static const struct hwmon_chip_info marvell_hwmon_chip_info = {
++	.ops = &marvell_hwmon_hwmon_ops,
++	.info = marvell_hwmon_info,
+ };
+ 
+ static int marvell_hwmon_name(struct phy_device *phydev)
+@@ -2531,49 +2438,48 @@ static int marvell_hwmon_name(struct phy
+ 	return 0;
+ }
+ 
+-static int marvell_hwmon_probe(struct phy_device *phydev,
+-			       const struct hwmon_chip_info *chip)
++static int marvell_hwmon_probe(struct phy_device *phydev)
+ {
++	const struct marvell_hwmon_ops *ops = to_marvell_hwmon_ops(phydev);
+ 	struct marvell_priv *priv = phydev->priv;
+ 	struct device *dev = &phydev->mdio.dev;
+ 	int err;
+ 
++	if (!ops)
++		return 0;
++
+ 	err = marvell_hwmon_name(phydev);
+ 	if (err)
+ 		return err;
+ 
+ 	priv->hwmon_dev = devm_hwmon_device_register_with_info(
+-		dev, priv->hwmon_name, phydev, chip, NULL);
++		dev, priv->hwmon_name, phydev, &marvell_hwmon_chip_info, NULL);
+ 
+ 	return PTR_ERR_OR_ZERO(priv->hwmon_dev);
+ }
+ 
+-static int m88e1121_hwmon_probe(struct phy_device *phydev)
+-{
+-	return marvell_hwmon_probe(phydev, &m88e1121_hwmon_chip_info);
+-}
++static const struct marvell_hwmon_ops m88e1121_hwmon_ops = {
++	.get_temp = m88e1121_get_temp,
++};
+ 
+-static int m88e1510_hwmon_probe(struct phy_device *phydev)
+-{
+-	return marvell_hwmon_probe(phydev, &m88e1510_hwmon_chip_info);
+-}
++static const struct marvell_hwmon_ops m88e1510_hwmon_ops = {
++	.get_temp = m88e1510_get_temp,
++	.get_temp_critical = m88e1510_get_temp_critical,
++	.set_temp_critical = m88e1510_set_temp_critical,
++	.get_temp_alarm = m88e1510_get_temp_alarm,
++};
++
++static const struct marvell_hwmon_ops m88e6390_hwmon_ops = {
++	.get_temp = m88e6390_get_temp,
++};
++
++#define DEF_MARVELL_HWMON_OPS(s) (&(s))
+ 
+-static int m88e6390_hwmon_probe(struct phy_device *phydev)
+-{
+-	return marvell_hwmon_probe(phydev, &m88e6390_hwmon_chip_info);
+-}
+ #else
+-static int m88e1121_hwmon_probe(struct phy_device *phydev)
+-{
+-	return 0;
+-}
+ 
+-static int m88e1510_hwmon_probe(struct phy_device *phydev)
+-{
+-	return 0;
+-}
++#define DEF_MARVELL_HWMON_OPS(s) NULL
+ 
+-static int m88e6390_hwmon_probe(struct phy_device *phydev)
++static int marvell_hwmon_probe(struct phy_device *phydev)
+ {
+ 	return 0;
+ }
+@@ -2589,40 +2495,7 @@ static int marvell_probe(struct phy_devi
+ 
+ 	phydev->priv = priv;
+ 
+-	return 0;
+-}
+-
+-static int m88e1121_probe(struct phy_device *phydev)
+-{
+-	int err;
+-
+-	err = marvell_probe(phydev);
+-	if (err)
+-		return err;
+-
+-	return m88e1121_hwmon_probe(phydev);
+-}
+-
+-static int m88e1510_probe(struct phy_device *phydev)
+-{
+-	int err;
+-
+-	err = marvell_probe(phydev);
+-	if (err)
+-		return err;
+-
+-	return m88e1510_hwmon_probe(phydev);
+-}
+-
+-static int m88e6390_probe(struct phy_device *phydev)
+-{
+-	int err;
+-
+-	err = marvell_probe(phydev);
+-	if (err)
+-		return err;
+-
+-	return m88e6390_hwmon_probe(phydev);
++	return marvell_hwmon_probe(phydev);
+ }
+ 
+ static struct phy_driver marvell_drivers[] = {
+@@ -2707,8 +2580,9 @@ static struct phy_driver marvell_drivers
+ 		.phy_id = MARVELL_PHY_ID_88E1121R,
+ 		.phy_id_mask = MARVELL_PHY_ID_MASK,
+ 		.name = "Marvell 88E1121R",
++		.driver_data = DEF_MARVELL_HWMON_OPS(m88e1121_hwmon_ops),
+ 		/* PHY_GBIT_FEATURES */
+-		.probe = m88e1121_probe,
++		.probe = marvell_probe,
+ 		.config_init = marvell_config_init,
+ 		.config_aneg = m88e1121_config_aneg,
+ 		.read_status = marvell_read_status,
+@@ -2827,9 +2701,10 @@ static struct phy_driver marvell_drivers
+ 		.phy_id = MARVELL_PHY_ID_88E1510,
+ 		.phy_id_mask = MARVELL_PHY_ID_MASK,
+ 		.name = "Marvell 88E1510",
++		.driver_data = DEF_MARVELL_HWMON_OPS(m88e1510_hwmon_ops),
+ 		.features = PHY_GBIT_FIBRE_FEATURES,
+ 		.flags = PHY_POLL_CABLE_TEST,
+-		.probe = m88e1510_probe,
++		.probe = marvell_probe,
+ 		.config_init = m88e1510_config_init,
+ 		.config_aneg = m88e1510_config_aneg,
+ 		.read_status = marvell_read_status,
+@@ -2856,9 +2731,10 @@ static struct phy_driver marvell_drivers
+ 		.phy_id = MARVELL_PHY_ID_88E1540,
+ 		.phy_id_mask = MARVELL_PHY_ID_MASK,
+ 		.name = "Marvell 88E1540",
++		.driver_data = DEF_MARVELL_HWMON_OPS(m88e1510_hwmon_ops),
+ 		/* PHY_GBIT_FEATURES */
+ 		.flags = PHY_POLL_CABLE_TEST,
+-		.probe = m88e1510_probe,
++		.probe = marvell_probe,
+ 		.config_init = marvell_config_init,
+ 		.config_aneg = m88e1510_config_aneg,
+ 		.read_status = marvell_read_status,
+@@ -2882,7 +2758,8 @@ static struct phy_driver marvell_drivers
+ 		.phy_id = MARVELL_PHY_ID_88E1545,
+ 		.phy_id_mask = MARVELL_PHY_ID_MASK,
+ 		.name = "Marvell 88E1545",
+-		.probe = m88e1510_probe,
++		.driver_data = DEF_MARVELL_HWMON_OPS(m88e1510_hwmon_ops),
++		.probe = marvell_probe,
+ 		/* PHY_GBIT_FEATURES */
+ 		.flags = PHY_POLL_CABLE_TEST,
+ 		.config_init = marvell_config_init,
+@@ -2928,9 +2805,10 @@ static struct phy_driver marvell_drivers
+ 		.phy_id = MARVELL_PHY_ID_88E6341_FAMILY,
+ 		.phy_id_mask = MARVELL_PHY_ID_MASK,
+ 		.name = "Marvell 88E6341 Family",
++		.driver_data = DEF_MARVELL_HWMON_OPS(m88e1510_hwmon_ops),
+ 		/* PHY_GBIT_FEATURES */
+ 		.flags = PHY_POLL_CABLE_TEST,
+-		.probe = m88e1510_probe,
++		.probe = marvell_probe,
+ 		.config_init = marvell_config_init,
+ 		.config_aneg = m88e6390_config_aneg,
+ 		.read_status = marvell_read_status,
+@@ -2954,9 +2832,10 @@ static struct phy_driver marvell_drivers
+ 		.phy_id = MARVELL_PHY_ID_88E6390_FAMILY,
+ 		.phy_id_mask = MARVELL_PHY_ID_MASK,
+ 		.name = "Marvell 88E6390 Family",
++		.driver_data = DEF_MARVELL_HWMON_OPS(m88e6390_hwmon_ops),
+ 		/* PHY_GBIT_FEATURES */
+ 		.flags = PHY_POLL_CABLE_TEST,
+-		.probe = m88e6390_probe,
++		.probe = marvell_probe,
+ 		.config_init = marvell_config_init,
+ 		.config_aneg = m88e6390_config_aneg,
+ 		.read_status = marvell_read_status,
+@@ -2980,7 +2859,8 @@ static struct phy_driver marvell_drivers
+ 		.phy_id = MARVELL_PHY_ID_88E1340S,
+ 		.phy_id_mask = MARVELL_PHY_ID_MASK,
+ 		.name = "Marvell 88E1340S",
+-		.probe = m88e1510_probe,
++		.driver_data = DEF_MARVELL_HWMON_OPS(m88e1510_hwmon_ops),
++		.probe = marvell_probe,
+ 		/* PHY_GBIT_FEATURES */
+ 		.config_init = marvell_config_init,
+ 		.config_aneg = m88e1510_config_aneg,
+@@ -3002,7 +2882,8 @@ static struct phy_driver marvell_drivers
+ 		.phy_id = MARVELL_PHY_ID_88E1548P,
+ 		.phy_id_mask = MARVELL_PHY_ID_MASK,
+ 		.name = "Marvell 88E1548P",
+-		.probe = m88e1510_probe,
++		.driver_data = DEF_MARVELL_HWMON_OPS(m88e1510_hwmon_ops),
++		.probe = marvell_probe,
+ 		.features = PHY_GBIT_FIBRE_FEATURES,
+ 		.config_init = marvell_config_init,
+ 		.config_aneg = m88e1510_config_aneg,
diff --git a/target/linux/generic/backport-5.10/743-v5.15-net-phy-marvell-add-SFP-support-for-88E1510.patch b/target/linux/generic/backport-5.10/743-v5.15-net-phy-marvell-add-SFP-support-for-88E1510.patch
new file mode 100644
index 0000000000..7fb78089f2
--- /dev/null
+++ b/target/linux/generic/backport-5.10/743-v5.15-net-phy-marvell-add-SFP-support-for-88E1510.patch
@@ -0,0 +1,161 @@
+From b697d9d38a5a5ab405d7cc4743d39fe2c5d7517c Mon Sep 17 00:00:00 2001
+From: Ivan Bornyakov <i.bornyakov@metrotek.ru>
+Date: Thu, 12 Aug 2021 16:42:56 +0300
+Subject: [PATCH] net: phy: marvell: add SFP support for 88E1510
+
+Add support for SFP cages connected to the Marvell 88E1512 transceiver.
+88E1512 supports for SGMII/1000Base-X/100Base-FX media type with RGMII
+on system interface. Configure PHY to appropriate mode depending on the
+type of SFP inserted. On SFP removal configure PHY to the RGMII-copper
+mode so RJ-45 port can still work.
+
+Signed-off-by: Ivan Bornyakov <i.bornyakov@metrotek.ru>
+Link: https://lore.kernel.org/r/20210812134256.2436-1-i.bornyakov@metrotek.ru
+Signed-off-by: Jakub Kicinski <kuba@kernel.org>
+---
+ drivers/net/phy/marvell.c | 105 +++++++++++++++++++++++++++++++++++++-
+ 1 file changed, 104 insertions(+), 1 deletion(-)
+
+--- a/drivers/net/phy/marvell.c
++++ b/drivers/net/phy/marvell.c
+@@ -32,6 +32,7 @@
+ #include <linux/marvell_phy.h>
+ #include <linux/bitfield.h>
+ #include <linux/of.h>
++#include <linux/sfp.h>
+ 
+ #include <linux/io.h>
+ #include <asm/irq.h>
+@@ -46,6 +47,7 @@
+ #define MII_MARVELL_MISC_TEST_PAGE	0x06
+ #define MII_MARVELL_VCT7_PAGE		0x07
+ #define MII_MARVELL_WOL_PAGE		0x11
++#define MII_MARVELL_MODE_PAGE		0x12
+ 
+ #define MII_M1011_IEVENT		0x13
+ #define MII_M1011_IEVENT_CLEAR		0x0000
+@@ -162,7 +164,14 @@
+ 
+ #define MII_88E1510_GEN_CTRL_REG_1		0x14
+ #define MII_88E1510_GEN_CTRL_REG_1_MODE_MASK	0x7
++#define MII_88E1510_GEN_CTRL_REG_1_MODE_RGMII	0x0	/* RGMII to copper */
+ #define MII_88E1510_GEN_CTRL_REG_1_MODE_SGMII	0x1	/* SGMII to copper */
++/* RGMII to 1000BASE-X */
++#define MII_88E1510_GEN_CTRL_REG_1_MODE_RGMII_1000X	0x2
++/* RGMII to 100BASE-FX */
++#define MII_88E1510_GEN_CTRL_REG_1_MODE_RGMII_100FX	0x3
++/* RGMII to SGMII */
++#define MII_88E1510_GEN_CTRL_REG_1_MODE_RGMII_SGMII	0x4
+ #define MII_88E1510_GEN_CTRL_REG_1_RESET	0x8000	/* Soft reset */
+ 
+ #define MII_VCT5_TX_RX_MDI0_COUPLING	0x10
+@@ -2498,6 +2507,100 @@ static int marvell_probe(struct phy_devi
+ 	return marvell_hwmon_probe(phydev);
+ }
+ 
++static int m88e1510_sfp_insert(void *upstream, const struct sfp_eeprom_id *id)
++{
++	struct phy_device *phydev = upstream;
++	phy_interface_t interface;
++	struct device *dev;
++	int oldpage;
++	int ret = 0;
++	u16 mode;
++
++	__ETHTOOL_DECLARE_LINK_MODE_MASK(supported) = { 0, };
++
++	dev = &phydev->mdio.dev;
++
++	sfp_parse_support(phydev->sfp_bus, id, supported);
++	interface = sfp_select_interface(phydev->sfp_bus, supported);
++
++	dev_info(dev, "%s SFP module inserted\n", phy_modes(interface));
++
++	switch (interface) {
++	case PHY_INTERFACE_MODE_1000BASEX:
++		mode = MII_88E1510_GEN_CTRL_REG_1_MODE_RGMII_1000X;
++
++		break;
++	case PHY_INTERFACE_MODE_100BASEX:
++		mode = MII_88E1510_GEN_CTRL_REG_1_MODE_RGMII_100FX;
++
++		break;
++	case PHY_INTERFACE_MODE_SGMII:
++		mode = MII_88E1510_GEN_CTRL_REG_1_MODE_RGMII_SGMII;
++
++		break;
++	default:
++		dev_err(dev, "Incompatible SFP module inserted\n");
++
++		return -EINVAL;
++	}
++
++	oldpage = phy_select_page(phydev, MII_MARVELL_MODE_PAGE);
++	if (oldpage < 0)
++		goto error;
++
++	ret = __phy_modify(phydev, MII_88E1510_GEN_CTRL_REG_1,
++			   MII_88E1510_GEN_CTRL_REG_1_MODE_MASK, mode);
++	if (ret < 0)
++		goto error;
++
++	ret = __phy_set_bits(phydev, MII_88E1510_GEN_CTRL_REG_1,
++			     MII_88E1510_GEN_CTRL_REG_1_RESET);
++
++error:
++	return phy_restore_page(phydev, oldpage, ret);
++}
++
++static void m88e1510_sfp_remove(void *upstream)
++{
++	struct phy_device *phydev = upstream;
++	int oldpage;
++	int ret = 0;
++
++	oldpage = phy_select_page(phydev, MII_MARVELL_MODE_PAGE);
++	if (oldpage < 0)
++		goto error;
++
++	ret = __phy_modify(phydev, MII_88E1510_GEN_CTRL_REG_1,
++			   MII_88E1510_GEN_CTRL_REG_1_MODE_MASK,
++			   MII_88E1510_GEN_CTRL_REG_1_MODE_RGMII);
++	if (ret < 0)
++		goto error;
++
++	ret = __phy_set_bits(phydev, MII_88E1510_GEN_CTRL_REG_1,
++			     MII_88E1510_GEN_CTRL_REG_1_RESET);
++
++error:
++	phy_restore_page(phydev, oldpage, ret);
++}
++
++static const struct sfp_upstream_ops m88e1510_sfp_ops = {
++	.module_insert = m88e1510_sfp_insert,
++	.module_remove = m88e1510_sfp_remove,
++	.attach = phy_sfp_attach,
++	.detach = phy_sfp_detach,
++};
++
++static int m88e1510_probe(struct phy_device *phydev)
++{
++	int err;
++
++	err = marvell_probe(phydev);
++	if (err)
++		return err;
++
++	return phy_sfp_probe(phydev, &m88e1510_sfp_ops);
++}
++
+ static struct phy_driver marvell_drivers[] = {
+ 	{
+ 		.phy_id = MARVELL_PHY_ID_88E1101,
+@@ -2704,7 +2807,7 @@ static struct phy_driver marvell_drivers
+ 		.driver_data = DEF_MARVELL_HWMON_OPS(m88e1510_hwmon_ops),
+ 		.features = PHY_GBIT_FIBRE_FEATURES,
+ 		.flags = PHY_POLL_CABLE_TEST,
+-		.probe = marvell_probe,
++		.probe = m88e1510_probe,
+ 		.config_init = m88e1510_config_init,
+ 		.config_aneg = m88e1510_config_aneg,
+ 		.read_status = marvell_read_status,