net: dsa: bcm_sf2: recalculate switch clock rate based on ports

Whenever a port gets enabled/disabled, recalcultate the required switch
clock rate to make sure it always gets set to the expected rate
targeting our switch use case. This is only done for the BCM7445 switch
as there is no clocking profile available for BCM7278.

Signed-off-by: Florian Fainelli <f.fainelli@gmail.com>
Reviewed-by: Andrew Lunn <andrew@lunn.ch>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/net/dsa/bcm_sf2.c b/drivers/net/dsa/bcm_sf2.c
index b8fa0a46c5c974a40fb272b89ff809b06649ac39..1c7fbb6f0447fec8e48bd41db1ce45fd43d60b87 100644 (file)
--- a/drivers/net/dsa/bcm_sf2.c
+++ b/drivers/net/dsa/bcm_sf2.c
@@ -32,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);
@@ -83,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)
@@ -168,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);
@@ -261,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);
 }
 
 
@@ -1202,10 +1255,18 @@ static int bcm_sf2_sw_probe(struct platform_device *pdev)
 
        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);
-               goto out_clk;
+               goto out_clk_mdiv;
        }
 
        bcm_sf2_gphy_enable_set(priv->dev->ds, true);
@@ -1213,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");
-               goto out_clk;
+               goto out_clk_mdiv;
        }
 
        bcm_sf2_gphy_enable_set(priv->dev->ds, false);
@@ -1280,6 +1341,8 @@ 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;
@@ -1295,6 +1358,7 @@ 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);

diff --git a/drivers/net/dsa/bcm_sf2.h b/drivers/net/dsa/bcm_sf2.h
index 6dd69922e3f60c980f1a1eab282e23640d169b14..1ed901a6853617f827eb2d60ea9570c3a74948d9 100644 (file)
--- a/drivers/net/dsa/bcm_sf2.h
+++ b/drivers/net/dsa/bcm_sf2.h
@@ -45,6 +45,7 @@ struct bcm_sf2_hw_params {
 
 struct bcm_sf2_port_status {
        unsigned int link;
+       bool enabled;
 };
 
 struct bcm_sf2_cfp_priv {
@@ -94,6 +95,7 @@ struct bcm_sf2_priv {
        u32                             wol_ports_mask;
 
        struct clk                      *clk;
+       struct clk                      *clk_mdiv;
 
        /* MoCA port location */
        int                             moca_port;