dsa: mv88e6xxx: Allow speed/duplex of port to be configured

[karo-tx-linux.git] / drivers / net / dsa / mv88e6xxx.c

diff --git a/drivers/net/dsa/mv88e6xxx.c b/drivers/net/dsa/mv88e6xxx.c
index 3774f53d28d781aaec7f5150352a944ba505d974..1a8c45f3e68057d1b9c8f8044c618bc9b68f7425 100644 (file)
--- a/drivers/net/dsa/mv88e6xxx.c
+++ b/drivers/net/dsa/mv88e6xxx.c
@@ -14,6 +14,7 @@
 #include <linux/debugfs.h>
 #include <linux/delay.h>
 #include <linux/etherdevice.h>
+#include <linux/ethtool.h>
 #include <linux/if_bridge.h>
 #include <linux/jiffies.h>
 #include <linux/list.h>
@@ -560,6 +561,63 @@ static bool mv88e6xxx_6352_family(struct dsa_switch *ds)
        return false;
 }
 
+/* We expect the switch to perform auto negotiation if there is a real
+ * phy. However, in the case of a fixed link phy, we force the port
+ * settings from the fixed link settings.
+ */
+void mv88e6xxx_adjust_link(struct dsa_switch *ds, int port,
+                          struct phy_device *phydev)
+{
+       struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+       u32 ret, reg;
+
+       if (!phy_is_pseudo_fixed_link(phydev))
+               return;
+
+       mutex_lock(&ps->smi_mutex);
+
+       ret = _mv88e6xxx_reg_read(ds, REG_PORT(port), PORT_PCS_CTRL);
+       if (ret < 0)
+               goto out;
+
+       reg = ret & ~(PORT_PCS_CTRL_LINK_UP |
+                     PORT_PCS_CTRL_FORCE_LINK |
+                     PORT_PCS_CTRL_DUPLEX_FULL |
+                     PORT_PCS_CTRL_FORCE_DUPLEX |
+                     PORT_PCS_CTRL_UNFORCED);
+
+       reg |= PORT_PCS_CTRL_FORCE_LINK;
+       if (phydev->link)
+                       reg |= PORT_PCS_CTRL_LINK_UP;
+
+       if (mv88e6xxx_6065_family(ds) && phydev->speed > SPEED_100)
+               goto out;
+
+       switch (phydev->speed) {
+       case SPEED_1000:
+               reg |= PORT_PCS_CTRL_1000;
+               break;
+       case SPEED_100:
+               reg |= PORT_PCS_CTRL_100;
+               break;
+       case SPEED_10:
+               reg |= PORT_PCS_CTRL_10;
+               break;
+       default:
+               pr_info("Unknown speed");
+               goto out;
+       }
+
+       reg |= PORT_PCS_CTRL_FORCE_DUPLEX;
+       if (phydev->duplex == DUPLEX_FULL)
+               reg |= PORT_PCS_CTRL_DUPLEX_FULL;
+
+       _mv88e6xxx_reg_write(ds, REG_PORT(port), PORT_PCS_CTRL, reg);
+
+out:
+       mutex_unlock(&ps->smi_mutex);
+}
+
 /* Must be called with SMI mutex held */
 static int _mv88e6xxx_stats_wait(struct dsa_switch *ds)
 {