[TG3]: Add 1000T & 1000X flowctrl resolvers

This patch adds two new utility functions to resolve flow control.  One
function resolves flow control based on 1000-BaseT register definitions.
The other resolves flow control based on 1000-Base X register
definitions.
Signed-off-by: Matt Carlson <mcarlson@broadcom.com>
Signed-off-by: Michael Chan <mchan@broadcom.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

drivers/net/tg3.c

@@ -1612,49 +1612,63 @@ static void tg3_link_report(struct tg3 *tp)
 	}
 }
 
-static void tg3_setup_flow_control(struct tg3 *tp, u32 local_adv, u32 remote_adv)
+static u8 tg3_resolve_flowctrl_1000T(u16 lcladv, u16 rmtadv)
 {
-	u8 new_tg3_flags = 0;
-	u32 old_rx_mode = tp->rx_mode;
-	u32 old_tx_mode = tp->tx_mode;
+	u8 cap = 0;
 
-	if (tp->tg3_flags & TG3_FLAG_PAUSE_AUTONEG) {
-
-		/* Convert 1000BaseX flow control bits to 1000BaseT
-		 * bits before resolving flow control.
-		 */
-		if (tp->tg3_flags2 & TG3_FLG2_MII_SERDES) {
-			local_adv &= ~(ADVERTISE_PAUSE_CAP |
-				       ADVERTISE_PAUSE_ASYM);
-			remote_adv &= ~(LPA_PAUSE_CAP | LPA_PAUSE_ASYM);
-
-			if (local_adv & ADVERTISE_1000XPAUSE)
-				local_adv |= ADVERTISE_PAUSE_CAP;
-			if (local_adv & ADVERTISE_1000XPSE_ASYM)
-				local_adv |= ADVERTISE_PAUSE_ASYM;
-			if (remote_adv & LPA_1000XPAUSE)
-				remote_adv |= LPA_PAUSE_CAP;
-			if (remote_adv & LPA_1000XPAUSE_ASYM)
-				remote_adv |= LPA_PAUSE_ASYM;
+	if (lcladv & ADVERTISE_PAUSE_CAP) {
+		if (lcladv & ADVERTISE_PAUSE_ASYM) {
+			if (rmtadv & LPA_PAUSE_CAP)
+				cap = TG3_FLOW_CTRL_TX | TG3_FLOW_CTRL_RX;
+			else if (rmtadv & LPA_PAUSE_ASYM)
+				cap = TG3_FLOW_CTRL_RX;
+		} else {
+			if (rmtadv & LPA_PAUSE_CAP)
+				cap = TG3_FLOW_CTRL_TX | TG3_FLOW_CTRL_RX;
+		}
+	} else if (lcladv & ADVERTISE_PAUSE_ASYM) {
+		if ((rmtadv & LPA_PAUSE_CAP) && (rmtadv & LPA_PAUSE_ASYM))
+			cap = TG3_FLOW_CTRL_TX;
 	}
 
-		if (local_adv & ADVERTISE_PAUSE_CAP) {
-			if (local_adv & ADVERTISE_PAUSE_ASYM) {
-				if (remote_adv & LPA_PAUSE_CAP)
-					new_tg3_flags = TG3_FLOW_CTRL_RX |
-							TG3_FLOW_CTRL_TX;
-				else if (remote_adv & LPA_PAUSE_ASYM)
-					new_tg3_flags = TG3_FLOW_CTRL_RX;
+	return cap;
+}
+
+static u8 tg3_resolve_flowctrl_1000X(u16 lcladv, u16 rmtadv)
+{
+	u8 cap = 0;
+
+	if (lcladv & ADVERTISE_1000XPAUSE) {
+		if (lcladv & ADVERTISE_1000XPSE_ASYM) {
+			if (rmtadv & LPA_1000XPAUSE)
+				cap = TG3_FLOW_CTRL_TX | TG3_FLOW_CTRL_RX;
+			else if (rmtadv & LPA_1000XPAUSE_ASYM)
+				cap = TG3_FLOW_CTRL_RX;
 		} else {
-				if (remote_adv & LPA_PAUSE_CAP)
-					new_tg3_flags = TG3_FLOW_CTRL_RX |
-							TG3_FLOW_CTRL_TX;
+			if (rmtadv & LPA_1000XPAUSE)
+				cap = TG3_FLOW_CTRL_TX | TG3_FLOW_CTRL_RX;
 		}
-		} else if (local_adv & ADVERTISE_PAUSE_ASYM) {
-			if ((remote_adv & LPA_PAUSE_CAP) &&
-			    (remote_adv & LPA_PAUSE_ASYM))
-				new_tg3_flags = TG3_FLOW_CTRL_TX;
+	} else if (lcladv & ADVERTISE_1000XPSE_ASYM) {
+		if ((rmtadv & LPA_1000XPAUSE) && (rmtadv & LPA_1000XPAUSE_ASYM))
+			cap = TG3_FLOW_CTRL_TX;
 	}
+
+	return cap;
+}
+
+static void tg3_setup_flow_control(struct tg3 *tp, u32 local_adv, u32 remote_adv)
+{
+	u8 new_tg3_flags = 0;
+	u32 old_rx_mode = tp->rx_mode;
+	u32 old_tx_mode = tp->tx_mode;
+
+	if (tp->tg3_flags & TG3_FLAG_PAUSE_AUTONEG) {
+		if (tp->tg3_flags2 & TG3_FLG2_MII_SERDES)
+			new_tg3_flags = tg3_resolve_flowctrl_1000X(local_adv,
+								   remote_adv);
+		else
+			new_tg3_flags = tg3_resolve_flowctrl_1000T(local_adv,
+								   remote_adv);
 	} else {
 		new_tg3_flags = tp->link_config.flowctrl;
 	}