dpaa_eth: add MAC PAUSE frames autonegotiation support

PHY devices advertise symmetric/asymmetric PAUSE frame capabilities to
their peers.
This patch changes the 'adjust_link' function behavior. When a
change occurs in the advertised or remote capabilities and PAUSE frame
autonegotiation is enabled, the FMan port adjusts PAUSE frames
reception and transmission depending on these settings.
When PAUSE autonegotiation is disabled, the FMan interface adjusts
PAUSE frames RX/TX depending on the values in the 'mac_device'.

Signed-off-by: Cristian Bercaru <cristian.bercaru@freescale.com>
Change-Id: I360ad283e2820d9f681f66606abf073f1c0708a9
Reviewed-on: http://git.am.freescale.net:8181/8721
Tested-by: Review Code-CDREVIEW <CDREVIEW@freescale.com>
Reviewed-by: Madalin-Cristian Bucur <madalin.bucur@freescale.com>
Reviewed-by: Jose Rivera <German.Rivera@freescale.com>

3 files changed, 117 insertions, 2 deletions

diff --git a/drivers/net/ethernet/freescale/dpa/mac-api.c b/drivers/net/ethernet/freescale/dpa/mac-api.c
index 56104dc..4105e1d 100644
--- a/drivers/net/ethernet/freescale/dpa/mac-api.c
+++ b/drivers/net/ethernet/freescale/dpa/mac-api.c
@@ -329,14 +329,110 @@ static int __cold set_multi(struct net_device *net_dev,
 	return 0;
 }
 
+/* Avoid redundant calls to FMD, if the MAC driver already contains the desired
+ * settings. Otherwise, the new MAC settings should be reflected in FMan.
+ */
+int set_mac_rx_pause(struct mac_device *mac_dev,
+		struct fm_mac_dev *fm_mac_dev, bool en)
+{
+	int _errno = 0;
+
+	if (unlikely(en != mac_dev->rx_pause)) {
+		_errno = mac_dev->set_rx_pause(fm_mac_dev, en);
+		if (likely(_errno == 0))
+			mac_dev->rx_pause = en;
+	}
+
+	return _errno;
+}
+
+int set_mac_tx_pause(struct mac_device *mac_dev,
+		struct fm_mac_dev *fm_mac_dev, bool en)
+{
+	int _errno = 0;
+
+	if (unlikely(en != mac_dev->tx_pause)) {
+		_errno = mac_dev->set_tx_pause(fm_mac_dev, en);
+		if (likely(_errno == 0))
+			mac_dev->tx_pause = en;
+	}
+
+	return _errno;
+}
+
+/* Determine the MAC RX/TX PAUSE frames settings based on PHY
+ * autonegotiation or values set by eththool.
+ */
+static void get_pause_cfg(struct mac_device *mac_dev,
+		bool *rx_pause, bool *tx_pause)
+{
+	struct phy_device *phy_dev = mac_dev->phy_dev;
+	u16 lcl_adv, rmt_adv;
+	u8 flowctrl;
+
+	*rx_pause = *tx_pause = false;
+
+	if (!phy_dev->duplex)
+		return;
+
+	/* If PAUSE autonegotiation is disabled, the TX/RX PAUSE settings
+	 * are those set by ethtool.
+	 */
+	if (!mac_dev->autoneg_pause) {
+		*rx_pause = !!mac_dev->rx_pause;
+		*tx_pause = !!mac_dev->tx_pause;
+		return;
+	}
+
+	/* Else if PAUSE autonegotiation is enabled, the TX/RX PAUSE
+	 * settings depend on the result of the link negotiation.
+	 */
+
+	/* get local capabilities */
+	lcl_adv = 0;
+	if (phy_dev->advertising & ADVERTISED_Pause)
+		lcl_adv |= ADVERTISE_PAUSE_CAP;
+	if (phy_dev->advertising & ADVERTISED_Asym_Pause)
+		lcl_adv |= ADVERTISE_PAUSE_ASYM;
+
+	/* get link partner capabilities */
+	rmt_adv = 0;
+	if (phy_dev->pause)
+		rmt_adv |= LPA_PAUSE_CAP;
+	if (phy_dev->asym_pause)
+		rmt_adv |= LPA_PAUSE_ASYM;
+
+	/* Calculate TX/RX settings based on local and peer advertised
+	 * symmetric/asymmetric PAUSE capabilities.
+	 */
+	flowctrl = mii_resolve_flowctrl_fdx(lcl_adv, rmt_adv);
+	if (flowctrl & FLOW_CTRL_RX)
+		*rx_pause = true;
+	if (flowctrl & FLOW_CTRL_TX)
+		*tx_pause = true;
+}
+
 static void adjust_link(struct net_device *net_dev)
 {
 	struct dpa_priv_s *priv = netdev_priv(net_dev);
 	struct mac_device *mac_dev = priv->mac_dev;
 	struct phy_device *phy_dev = mac_dev->phy_dev;
+	struct fm_mac_dev *fm_mac_dev;
+	bool rx_pause, tx_pause;
+	int _errno;
+
+	fm_mac_dev = mac_dev->get_mac_handle(mac_dev);
+	fm_mac_adjust_link(fm_mac_dev, phy_dev->link, phy_dev->speed,
+			phy_dev->duplex);
 
-	fm_mac_adjust_link(mac_dev->get_mac_handle(mac_dev),
-			phy_dev->link, phy_dev->speed, phy_dev->duplex);
+	get_pause_cfg(mac_dev, &rx_pause, &tx_pause);
+
+	_errno = set_mac_rx_pause(mac_dev, fm_mac_dev, rx_pause);
+	if (unlikely(_errno < 0))
+		netdev_err(net_dev, "set_rx_pause() = %d\n", _errno);
+	_errno = set_mac_tx_pause(mac_dev, fm_mac_dev, tx_pause);
+	if (unlikely(_errno < 0))
+		netdev_err(net_dev, "set_tx_pause() = %d\n", _errno);
 }
 
 /* Initializes driver's PHY state, and attaches to the PHY.
@@ -363,6 +459,10 @@ static int dtsec_init_phy(struct net_device *net_dev,
 
 	/* Remove any features not supported by the controller */
 	phy_dev->supported &= mac_dev->if_support;
+	/* Enable the symmetric and asymmetric PAUSE frame advertisments,
+	 * as most of the PHY drivers do not enable them by default.
+	 */
+	phy_dev->supported |= (SUPPORTED_Pause | SUPPORTED_Asym_Pause);
 	phy_dev->advertising = phy_dev->supported;
 
 	mac_dev->phy_dev = phy_dev;
@@ -390,6 +490,10 @@ static int xgmac_init_phy(struct net_device *net_dev,
 	}
 
 	phy_dev->supported &= mac_dev->if_support;
+	/* Enable the symmetric and asymmetric PAUSE frame advertisments,
+	 * as most of the PHY drivers do not enable them by default.
+	 */
+	phy_dev->supported |= (SUPPORTED_Pause | SUPPORTED_Asym_Pause);
 	phy_dev->advertising = phy_dev->supported;
 
 	mac_dev->phy_dev = phy_dev;
@@ -429,6 +533,10 @@ static int memac_init_phy(struct net_device *net_dev,
 
 	/* Remove any features not supported by the controller */
 	phy_dev->supported &= mac_dev->if_support;
+	/* Enable the symmetric and asymmetric PAUSE frame advertisments,
+	 * as most of the PHY drivers do not enable them by default.
+	 */
+	phy_dev->supported |= (SUPPORTED_Pause | SUPPORTED_Asym_Pause);
 	phy_dev->advertising = phy_dev->supported;
 
 	mac_dev->phy_dev = phy_dev;
diff --git a/drivers/net/ethernet/freescale/dpa/mac.c b/drivers/net/ethernet/freescale/dpa/mac.c
index bc6360c..aaeeb5e 100644
--- a/drivers/net/ethernet/freescale/dpa/mac.c
+++ b/drivers/net/ethernet/freescale/dpa/mac.c
@@ -56,6 +56,8 @@
 	| SUPPORTED_100baseT_Half \
 	| SUPPORTED_100baseT_Full \
 	| SUPPORTED_Autoneg \
+	| SUPPORTED_Pause \
+	| SUPPORTED_Asym_Pause \
 	| SUPPORTED_MII)
 
 static const char phy_str[][11] = {
diff --git a/drivers/net/ethernet/freescale/dpa/mac.h b/drivers/net/ethernet/freescale/dpa/mac.h
index edbed2a..3aa3c65 100644
--- a/drivers/net/ethernet/freescale/dpa/mac.h
+++ b/drivers/net/ethernet/freescale/dpa/mac.h
@@ -119,4 +119,9 @@ extern const char	*mac_driver_description;
 extern const size_t	 mac_sizeof_priv[];
 extern void (*const mac_setup[])(struct mac_device *mac_dev);
 
+int set_mac_rx_pause(struct mac_device *mac_dev,
+		struct fm_mac_dev *fm_mac_dev, bool en);
+int set_mac_tx_pause(struct mac_device *mac_dev,
+		struct fm_mac_dev *fm_mac_dev, bool en);
+
 #endif	/* __MAC_H */