diff --git a/drivers/net/phy/mscc/mscc_main.c b/drivers/net/phy/mscc/mscc_main.c index 032050ec0bc9..052e68d1cd97 100644 --- a/drivers/net/phy/mscc/mscc_main.c +++ b/drivers/net/phy/mscc/mscc_main.c @@ -22,6 +22,24 @@ #include "mscc_serdes.h" #include "mscc.h" +struct vsc85xx_probe_config { + const struct vsc85xx_hw_stat *hw_stats; + size_t shared_size; + size_t nstats; + u16 supp_led_modes; + u8 nleds; + bool check_rate_magic; + bool use_package; + bool has_ptp; +}; + +static const u32 vsc85xx_default_led_modes_4[] = { + VSC8531_LINK_1000_ACTIVITY, + VSC8531_LINK_100_ACTIVITY, + VSC8531_LINK_ACTIVITY, + VSC8531_DUPLEX_COLLISION +}; + static const struct vsc85xx_hw_stat vsc85xx_hw_stats[] = { { .string = "phy_receive_errors", @@ -436,7 +454,7 @@ static int vsc85xx_dt_led_mode_get(struct phy_device *phydev, #endif /* CONFIG_OF_MDIO */ static int vsc85xx_dt_led_modes_get(struct phy_device *phydev, - u32 *default_mode) + const u32 *default_mode) { struct vsc8531_private *priv = phydev->priv; char led_dt_prop[28]; @@ -2211,70 +2229,11 @@ static int vsc85xx_config_inband(struct phy_device *phydev, unsigned int modes) reg_val); } -static int vsc8514_probe(struct phy_device *phydev) +static int vsc85xx_probe_common(struct phy_device *phydev, + const struct vsc85xx_probe_config *cfg, + const u32 *default_led_mode) { struct vsc8531_private *vsc8531; - u32 default_mode[4] = {VSC8531_LINK_1000_ACTIVITY, - VSC8531_LINK_100_ACTIVITY, VSC8531_LINK_ACTIVITY, - VSC8531_DUPLEX_COLLISION}; - - vsc8531 = devm_kzalloc(&phydev->mdio.dev, sizeof(*vsc8531), GFP_KERNEL); - if (!vsc8531) - return -ENOMEM; - - phydev->priv = vsc8531; - - vsc8584_get_base_addr(phydev); - devm_phy_package_join(&phydev->mdio.dev, phydev, - vsc8531->base_addr, 0); - - vsc8531->nleds = 4; - vsc8531->supp_led_modes = VSC85XX_SUPP_LED_MODES; - vsc8531->hw_stats = vsc85xx_hw_stats; - vsc8531->nstats = ARRAY_SIZE(vsc85xx_hw_stats); - vsc8531->stats = devm_kcalloc(&phydev->mdio.dev, vsc8531->nstats, - sizeof(u64), GFP_KERNEL); - if (!vsc8531->stats) - return -ENOMEM; - - return vsc85xx_dt_led_modes_get(phydev, default_mode); -} - -static int vsc8574_probe(struct phy_device *phydev) -{ - struct vsc8531_private *vsc8531; - u32 default_mode[4] = {VSC8531_LINK_1000_ACTIVITY, - VSC8531_LINK_100_ACTIVITY, VSC8531_LINK_ACTIVITY, - VSC8531_DUPLEX_COLLISION}; - - vsc8531 = devm_kzalloc(&phydev->mdio.dev, sizeof(*vsc8531), GFP_KERNEL); - if (!vsc8531) - return -ENOMEM; - - phydev->priv = vsc8531; - - vsc8584_get_base_addr(phydev); - devm_phy_package_join(&phydev->mdio.dev, phydev, - vsc8531->base_addr, 0); - - vsc8531->nleds = 4; - vsc8531->supp_led_modes = VSC8584_SUPP_LED_MODES; - vsc8531->hw_stats = vsc8584_hw_stats; - vsc8531->nstats = ARRAY_SIZE(vsc8584_hw_stats); - vsc8531->stats = devm_kcalloc(&phydev->mdio.dev, vsc8531->nstats, - sizeof(u64), GFP_KERNEL); - if (!vsc8531->stats) - return -ENOMEM; - - return vsc85xx_dt_led_modes_get(phydev, default_mode); -} - -static int vsc8584_probe(struct phy_device *phydev) -{ - struct vsc8531_private *vsc8531; - u32 default_mode[4] = {VSC8531_LINK_1000_ACTIVITY, - VSC8531_LINK_100_ACTIVITY, VSC8531_LINK_ACTIVITY, - VSC8531_DUPLEX_COLLISION}; int ret; vsc8531 = devm_kzalloc(&phydev->mdio.dev, sizeof(*vsc8531), GFP_KERNEL); @@ -2283,60 +2242,112 @@ static int vsc8584_probe(struct phy_device *phydev) phydev->priv = vsc8531; - vsc8584_get_base_addr(phydev); - devm_phy_package_join(&phydev->mdio.dev, phydev, vsc8531->base_addr, - sizeof(struct vsc85xx_shared_private)); + /* Check rate magic if needed (only for non-package PHYs) */ + if (cfg->check_rate_magic) { + ret = vsc85xx_edge_rate_magic_get(phydev); + if (ret < 0) + return ret; - vsc8531->nleds = 4; - vsc8531->supp_led_modes = VSC8584_SUPP_LED_MODES; - vsc8531->hw_stats = vsc8584_hw_stats; - vsc8531->nstats = ARRAY_SIZE(vsc8584_hw_stats); + vsc8531->rate_magic = ret; + } + + /* Set up package if needed */ + if (cfg->use_package) { + vsc8584_get_base_addr(phydev); + devm_phy_package_join(&phydev->mdio.dev, phydev, + vsc8531->base_addr, cfg->shared_size); + } + + /* Configure LED settings */ + vsc8531->nleds = cfg->nleds; + vsc8531->supp_led_modes = cfg->supp_led_modes; + + /* Configure hardware stats */ + vsc8531->hw_stats = cfg->hw_stats; + vsc8531->nstats = cfg->nstats; vsc8531->stats = devm_kcalloc(&phydev->mdio.dev, vsc8531->nstats, sizeof(u64), GFP_KERNEL); if (!vsc8531->stats) return -ENOMEM; - if (phy_package_probe_once(phydev)) { - ret = vsc8584_ptp_probe_once(phydev); + /* PTP setup for VSC8584 */ + if (cfg->has_ptp) { + if (phy_package_probe_once(phydev)) { + ret = vsc8584_ptp_probe_once(phydev); + if (ret) + return ret; + } + + ret = vsc8584_ptp_probe(phydev); if (ret) return ret; } - ret = vsc8584_ptp_probe(phydev); - if (ret) - return ret; + /* Parse LED modes from device tree */ + return vsc85xx_dt_led_modes_get(phydev, default_led_mode); +} - return vsc85xx_dt_led_modes_get(phydev, default_mode); +static int vsc8514_probe(struct phy_device *phydev) +{ + static const struct vsc85xx_probe_config vsc8514_cfg = { + .nleds = 4, + .supp_led_modes = VSC85XX_SUPP_LED_MODES, + .hw_stats = vsc85xx_hw_stats, + .nstats = ARRAY_SIZE(vsc85xx_hw_stats), + .use_package = true, + .shared_size = 0, + .has_ptp = false, + .check_rate_magic = false, + }; + + return vsc85xx_probe_common(phydev, &vsc8514_cfg, vsc85xx_default_led_modes_4); +} + +static int vsc8574_probe(struct phy_device *phydev) +{ + static const struct vsc85xx_probe_config vsc8574_cfg = { + .nleds = 4, + .supp_led_modes = VSC8584_SUPP_LED_MODES, + .hw_stats = vsc8584_hw_stats, + .nstats = ARRAY_SIZE(vsc8584_hw_stats), + .use_package = true, + .shared_size = 0, + .has_ptp = false, + .check_rate_magic = false, + }; + + return vsc85xx_probe_common(phydev, &vsc8574_cfg, vsc85xx_default_led_modes_4); +} + +static int vsc8584_probe(struct phy_device *phydev) +{ + static const struct vsc85xx_probe_config vsc8584_cfg = { + .nleds = 4, + .supp_led_modes = VSC8584_SUPP_LED_MODES, + .hw_stats = vsc8584_hw_stats, + .nstats = ARRAY_SIZE(vsc8584_hw_stats), + .use_package = true, + .shared_size = sizeof(struct vsc85xx_shared_private), + .has_ptp = true, + .check_rate_magic = false, + }; + + return vsc85xx_probe_common(phydev, &vsc8584_cfg, vsc85xx_default_led_modes_4); } static int vsc85xx_probe(struct phy_device *phydev) { - struct vsc8531_private *vsc8531; - int rate_magic; - u32 default_mode[2] = {VSC8531_LINK_1000_ACTIVITY, - VSC8531_LINK_100_ACTIVITY}; + static const struct vsc85xx_probe_config vsc85xx_cfg = { + .nleds = 2, + .supp_led_modes = VSC85XX_SUPP_LED_MODES, + .hw_stats = vsc85xx_hw_stats, + .nstats = ARRAY_SIZE(vsc85xx_hw_stats), + .use_package = false, + .has_ptp = false, + .check_rate_magic = true, + }; - rate_magic = vsc85xx_edge_rate_magic_get(phydev); - if (rate_magic < 0) - return rate_magic; - - vsc8531 = devm_kzalloc(&phydev->mdio.dev, sizeof(*vsc8531), GFP_KERNEL); - if (!vsc8531) - return -ENOMEM; - - phydev->priv = vsc8531; - - vsc8531->rate_magic = rate_magic; - vsc8531->nleds = 2; - vsc8531->supp_led_modes = VSC85XX_SUPP_LED_MODES; - vsc8531->hw_stats = vsc85xx_hw_stats; - vsc8531->nstats = ARRAY_SIZE(vsc85xx_hw_stats); - vsc8531->stats = devm_kcalloc(&phydev->mdio.dev, vsc8531->nstats, - sizeof(u64), GFP_KERNEL); - if (!vsc8531->stats) - return -ENOMEM; - - return vsc85xx_dt_led_modes_get(phydev, default_mode); + return vsc85xx_probe_common(phydev, &vsc85xx_cfg, vsc85xx_default_led_modes_4); } static void vsc85xx_remove(struct phy_device *phydev)