diff --git a/drivers/phy/qualcomm/phy-qcom-sgmii-eth.c b/drivers/phy/qualcomm/phy-qcom-sgmii-eth.c index 5b1c82459c12..4ea3dce7719f 100644 --- a/drivers/phy/qualcomm/phy-qcom-sgmii-eth.c +++ b/drivers/phy/qualcomm/phy-qcom-sgmii-eth.c @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -286,6 +287,37 @@ static int qcom_dwmac_sgmii_phy_power_off(struct phy *phy) return 0; } +static int qcom_dwmac_sgmii_phy_speed(enum phy_mode mode, int submode) +{ + if (mode != PHY_MODE_ETHERNET) + return -EINVAL; + + if (submode == PHY_INTERFACE_MODE_SGMII || + submode == PHY_INTERFACE_MODE_1000BASEX) + return SPEED_1000; + + if (submode == PHY_INTERFACE_MODE_2500BASEX) + return SPEED_2500; + + return -EINVAL; +} + +static int qcom_dwmac_sgmii_phy_set_mode(struct phy *phy, enum phy_mode mode, + int submode) +{ + struct qcom_dwmac_sgmii_phy_data *data = phy_get_drvdata(phy); + int speed; + + speed = qcom_dwmac_sgmii_phy_speed(mode, submode); + if (speed < 0) + return speed; + + if (speed != data->speed) + data->speed = speed; + + return qcom_dwmac_sgmii_phy_calibrate(phy); +} + static int qcom_dwmac_sgmii_phy_set_speed(struct phy *phy, int speed) { struct qcom_dwmac_sgmii_phy_data *data = phy_get_drvdata(phy); @@ -296,10 +328,21 @@ static int qcom_dwmac_sgmii_phy_set_speed(struct phy *phy, int speed) return qcom_dwmac_sgmii_phy_calibrate(phy); } +static int qcom_dwmac_sgmii_phy_validate(struct phy *phy, enum phy_mode mode, + int submode, + union phy_configure_opts *opts) +{ + int ret = qcom_dwmac_sgmii_phy_speed(mode, submode); + + return ret < 0 ? ret : 0; +} + static const struct phy_ops qcom_dwmac_sgmii_phy_ops = { .power_on = qcom_dwmac_sgmii_phy_power_on, .power_off = qcom_dwmac_sgmii_phy_power_off, + .set_mode = qcom_dwmac_sgmii_phy_set_mode, .set_speed = qcom_dwmac_sgmii_phy_set_speed, + .validate = qcom_dwmac_sgmii_phy_validate, .calibrate = qcom_dwmac_sgmii_phy_calibrate, .owner = THIS_MODULE, };