diff options
Diffstat (limited to 'drivers/net/dsa/bcm_sf2.c')
-rw-r--r-- | drivers/net/dsa/bcm_sf2.c | 84 |
1 files changed, 82 insertions, 2 deletions
diff --git a/drivers/net/dsa/bcm_sf2.c b/drivers/net/dsa/bcm_sf2.c index 5ebff986a1ac..7a74e4d73415 100644 --- a/drivers/net/dsa/bcm_sf2.c +++ b/drivers/net/dsa/bcm_sf2.c @@ -14,6 +14,7 @@ #include <linux/phy_fixed.h> #include <linux/phylink.h> #include <linux/mii.h> +#include <linux/clk.h> #include <linux/of.h> #include <linux/of_irq.h> #include <linux/of_address.h> @@ -31,6 +32,49 @@ #include "b53/b53_priv.h" #include "b53/b53_regs.h" +/* Return the number of active ports, not counting the IMP (CPU) port */ +static unsigned int bcm_sf2_num_active_ports(struct dsa_switch *ds) +{ + struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds); + unsigned int port, count = 0; + + for (port = 0; port < ARRAY_SIZE(priv->port_sts); port++) { + if (dsa_is_cpu_port(ds, port)) + continue; + if (priv->port_sts[port].enabled) + count++; + } + + return count; +} + +static void bcm_sf2_recalc_clock(struct dsa_switch *ds) +{ + struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds); + unsigned long new_rate; + unsigned int ports_active; + /* Frequenty in Mhz */ + const unsigned long rate_table[] = { + 59220000, + 60820000, + 62500000, + 62500000, + }; + + ports_active = bcm_sf2_num_active_ports(ds); + if (ports_active == 0 || !priv->clk_mdiv) + return; + + /* If we overflow our table, just use the recommended operational + * frequency + */ + if (ports_active > ARRAY_SIZE(rate_table)) + new_rate = 90000000; + else + new_rate = rate_table[ports_active - 1]; + clk_set_rate(priv->clk_mdiv, new_rate); +} + static void bcm_sf2_imp_setup(struct dsa_switch *ds, int port) { struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds); @@ -82,6 +126,8 @@ static void bcm_sf2_imp_setup(struct dsa_switch *ds, int port) reg &= ~(RX_DIS | TX_DIS); core_writel(priv, reg, CORE_G_PCTL_PORT(port)); } + + priv->port_sts[port].enabled = true; } static void bcm_sf2_gphy_enable_set(struct dsa_switch *ds, bool enable) @@ -167,6 +213,10 @@ static int bcm_sf2_port_setup(struct dsa_switch *ds, int port, if (!dsa_is_user_port(ds, port)) return 0; + priv->port_sts[port].enabled = true; + + bcm_sf2_recalc_clock(ds); + /* Clear the memory power down */ reg = core_readl(priv, CORE_MEM_PSM_VDD_CTRL); reg &= ~P_TXQ_PSM_VDD(port); @@ -260,6 +310,10 @@ static void bcm_sf2_port_disable(struct dsa_switch *ds, int port) reg = core_readl(priv, CORE_MEM_PSM_VDD_CTRL); reg |= P_TXQ_PSM_VDD(port); core_writel(priv, reg, CORE_MEM_PSM_VDD_CTRL); + + priv->port_sts[port].enabled = false; + + bcm_sf2_recalc_clock(ds); } @@ -750,6 +804,9 @@ static int bcm_sf2_sw_suspend(struct dsa_switch *ds) bcm_sf2_port_disable(ds, port); } + if (!priv->wol_ports_mask) + clk_disable_unprepare(priv->clk); + return 0; } @@ -758,6 +815,9 @@ static int bcm_sf2_sw_resume(struct dsa_switch *ds) struct bcm_sf2_priv *priv = bcm_sf2_to_priv(ds); int ret; + if (!priv->wol_ports_mask) + clk_prepare_enable(priv->clk); + ret = bcm_sf2_sw_rst(priv); if (ret) { pr_err("%s: failed to software reset switch\n", __func__); @@ -1189,10 +1249,24 @@ static int bcm_sf2_sw_probe(struct platform_device *pdev) base++; } + priv->clk = devm_clk_get_optional(&pdev->dev, "sw_switch"); + if (IS_ERR(priv->clk)) + return PTR_ERR(priv->clk); + + clk_prepare_enable(priv->clk); + + priv->clk_mdiv = devm_clk_get_optional(&pdev->dev, "sw_switch_mdiv"); + if (IS_ERR(priv->clk_mdiv)) { + ret = PTR_ERR(priv->clk_mdiv); + goto out_clk; + } + + clk_prepare_enable(priv->clk_mdiv); + ret = bcm_sf2_sw_rst(priv); if (ret) { pr_err("unable to software reset switch: %d\n", ret); - return ret; + goto out_clk_mdiv; } bcm_sf2_gphy_enable_set(priv->dev->ds, true); @@ -1200,7 +1274,7 @@ static int bcm_sf2_sw_probe(struct platform_device *pdev) ret = bcm_sf2_mdio_register(ds); if (ret) { pr_err("failed to register MDIO bus\n"); - return ret; + goto out_clk_mdiv; } bcm_sf2_gphy_enable_set(priv->dev->ds, false); @@ -1267,6 +1341,10 @@ static int bcm_sf2_sw_probe(struct platform_device *pdev) out_mdio: bcm_sf2_mdio_unregister(priv); +out_clk_mdiv: + clk_disable_unprepare(priv->clk_mdiv); +out_clk: + clk_disable_unprepare(priv->clk); return ret; } @@ -1280,6 +1358,8 @@ static int bcm_sf2_sw_remove(struct platform_device *pdev) dsa_unregister_switch(priv->dev->ds); bcm_sf2_cfp_exit(priv->dev->ds); bcm_sf2_mdio_unregister(priv); + clk_disable_unprepare(priv->clk_mdiv); + clk_disable_unprepare(priv->clk); if (priv->type == BCM7278_DEVICE_ID && !IS_ERR(priv->rcdev)) reset_control_assert(priv->rcdev); |