[PATCH RFC 2/3] net: phy: marvell10g: Add host interface speed configuration

[Yoshihiro Shimoda <yoshihiro.shimoda.uh@xxxxxxxxxxx>]

Add support for selecting host speed mode. For now, only support
1000M bps.

Signed-off-by: Yoshihiro Shimoda <yoshihiro.shimoda.uh@xxxxxxxxxxx>
---
 drivers/net/phy/marvell10g.c | 23 +++++++++++++++++++++++
 1 file changed, 23 insertions(+)

diff --git a/drivers/net/phy/marvell10g.c b/drivers/net/phy/marvell10g.c
index 383a9c9f36e5..daf3242c6078 100644
--- a/drivers/net/phy/marvell10g.c
+++ b/drivers/net/phy/marvell10g.c
@@ -101,6 +101,10 @@ enum {
 	MV_AN_21X0_SERDES_CTRL2_AUTO_INIT_DIS	= BIT(13),
 	MV_AN_21X0_SERDES_CTRL2_RUN_INIT	= BIT(15),
 
+	MV_MOD_CONF		= 0xf000,
+	MV_MOD_CONF_SPEED_MASK	= 0x00c0,
+	MV_MOD_CONF_SPEED_1000	= BIT(7),
+
 	/* These registers appear at 0x800X and 0xa00X - the 0xa00X control
 	 * registers appear to set themselves to the 0x800X when AN is
 	 * restarted, but status registers appear readable from either.
@@ -147,6 +151,7 @@ struct mv3310_chip {
 	int (*get_mactype)(struct phy_device *phydev);
 	int (*set_mactype)(struct phy_device *phydev, int mactype);
 	int (*select_mactype)(unsigned long *interfaces);
+	int (*set_macspeed)(struct phy_device *phydev, int macspeed);
 	int (*init_interface)(struct phy_device *phydev, int mactype);
 
 #ifdef CONFIG_HWMON
@@ -644,6 +649,16 @@ static int mv2110_select_mactype(unsigned long *interfaces)
 		return -1;
 }
 
+static int mv2110_set_macspeed(struct phy_device *phydev, int macspeed)
+{
+	if (macspeed != SPEED_1000)
+		return -EOPNOTSUPP;
+
+	return phy_modify_mmd(phydev, MDIO_MMD_VEND2, MV_MOD_CONF,
+			      MV_MOD_CONF_SPEED_MASK,
+			      MV_MOD_CONF_SPEED_1000);
+}
+
 static int mv3310_get_mactype(struct phy_device *phydev)
 {
 	int mactype;
@@ -778,6 +793,13 @@ static int mv3310_config_init(struct phy_device *phydev)
 	if (err)
 		return err;
 
+	/* If host provided host mac speed, try to set the mac speed */
+	if (phydev->host_speed && chip->set_macspeed) {
+		err = chip->set_macspeed(phydev, phydev->host_speed);
+		if (err)
+			return err;
+	}
+
 	/* If host provided host supported interface modes, try to select the
 	 * best one
 	 */
@@ -1181,6 +1203,7 @@ static const struct mv3310_chip mv2110_type = {
 	.get_mactype = mv2110_get_mactype,
 	.set_mactype = mv2110_set_mactype,
 	.select_mactype = mv2110_select_mactype,
+	.set_macspeed = mv2110_set_macspeed,
 	.init_interface = mv2110_init_interface,
 
 #ifdef CONFIG_HWMON
-- 
2.25.1