1 files changed, 29 insertions, 10 deletions

diff --git a/drivers/net/dsa/mv88e6xxx/chip.c b/drivers/net/dsa/mv88e6xxx/chip.c
index 0b49d243e00b..37b649501500 100644
--- a/drivers/net/dsa/mv88e6xxx/chip.c
+++ b/drivers/net/dsa/mv88e6xxx/chip.c
@@ -449,9 +449,6 @@ static int mv88e6xxx_port_setup_mac(struct mv88e6xxx_chip *chip, int port,
 			goto restore_link;
 	}
 
-	if (speed == SPEED_MAX && chip->info->ops->port_max_speed_mode)
-		mode = chip->info->ops->port_max_speed_mode(port);
-
 	if (chip->info->ops->port_set_pause) {
 		err = chip->info->ops->port_set_pause(chip, port, pause);
 		if (err)
@@ -3280,28 +3277,51 @@ static int mv88e6xxx_setup_port(struct mv88e6xxx_chip *chip, int port)
 {
 	struct device_node *phy_handle = NULL;
 	struct dsa_switch *ds = chip->ds;
+	phy_interface_t mode;
 	struct dsa_port *dp;
-	int tx_amp;
+	int tx_amp, speed;
 	int err;
 	u16 reg;
 
 	chip->ports[port].chip = chip;
 	chip->ports[port].port = port;
 
+	dp = dsa_to_port(ds, port);
+
 	/* MAC Forcing register: don't force link, speed, duplex or flow control
 	 * state to any particular values on physical ports, but force the CPU
 	 * port and all DSA ports to their maximum bandwidth and full duplex.
 	 */
-	if (dsa_is_cpu_port(ds, port) || dsa_is_dsa_port(ds, port))
+	if (dsa_is_cpu_port(ds, port) || dsa_is_dsa_port(ds, port)) {
+		unsigned long caps = dp->pl_config.mac_capabilities;
+
+		if (chip->info->ops->port_max_speed_mode)
+			mode = chip->info->ops->port_max_speed_mode(port);
+		else
+			mode = PHY_INTERFACE_MODE_NA;
+
+		if (caps & MAC_10000FD)
+			speed = SPEED_10000;
+		else if (caps & MAC_5000FD)
+			speed = SPEED_5000;
+		else if (caps & MAC_2500FD)
+			speed = SPEED_2500;
+		else if (caps & MAC_1000)
+			speed = SPEED_1000;
+		else if (caps & MAC_100)
+			speed = SPEED_100;
+		else
+			speed = SPEED_10;
+
 		err = mv88e6xxx_port_setup_mac(chip, port, LINK_FORCED_UP,
-					       SPEED_MAX, DUPLEX_FULL,
-					       PAUSE_OFF,
-					       PHY_INTERFACE_MODE_NA);
-	else
+					       speed, DUPLEX_FULL,
+					       PAUSE_OFF, mode);
+	} else {
 		err = mv88e6xxx_port_setup_mac(chip, port, LINK_UNFORCED,
 					       SPEED_UNFORCED, DUPLEX_UNFORCED,
 					       PAUSE_ON,
 					       PHY_INTERFACE_MODE_NA);
+	}
 	if (err)
 		return err;
 
@@ -3473,7 +3493,6 @@ static int mv88e6xxx_setup_port(struct mv88e6xxx_chip *chip, int port)
 	}
 
 	if (chip->info->ops->serdes_set_tx_amplitude) {
-		dp = dsa_to_port(ds, port);
 		if (dp)
 			phy_handle = of_parse_phandle(dp->dn, "phy-handle", 0);