e1000e: ethtool setfeatures fixes + loopback

index a5bd7a3..a83a108 100644
@@ -5901,7 +5901,7 @@ static void e1000_eeprom_checks(struct e1000_adapter *adapter)
 }
 
 static int e1000_set_features(struct net_device *netdev,
-	netdev_features_t features)
+			      netdev_features_t features)
 {
 	struct e1000_adapter *adapter = netdev_priv(netdev);
 	netdev_features_t changed = features ^ netdev->features;

@@ -5913,12 +5913,19 @@ static int e1000_set_features(struct net_device *netdev,
 			 NETIF_F_RXCSUM)))
 		return 0;
 
+	/* We will make sure to return 1 below so that we can change
+	 * netdev->features on the fly from within the functions
+	 * called below in case of error, without core/dev.c
+	 * overriding it. */
+	netdev->features = features;
+
 	if (netif_running(netdev))
 		e1000e_reinit_locked(adapter);
 	else
 		e1000e_reset(adapter);
 
-	return 0;
+	return 1; /* tell net/core/dev.c not to override
+		   * netdev->features */
 }
 
 static const struct net_device_ops e1000e_netdev_ops = {

index 9fe18d1..786cf31 100644
@@ -485,6 +485,8 @@ extern const char e1000e_driver_version[];
 
 extern void e1000e_check_options(struct e1000_adapter *adapter);
 extern void e1000e_set_ethtool_ops(struct net_device *netdev);
+extern int e1000_loopback_setup(struct e1000_adapter *adapter);
+extern void e1000_loopback_cleanup(struct e1000_adapter *adapter);
 
 extern int e1000e_up(struct e1000_adapter *adapter);
 extern void e1000e_down(struct e1000_adapter *adapter);

index fb2c28e..259add2 100644
@@ -1410,7 +1410,7 @@ static int e1000_set_es2lan_mac_loopback(struct e1000_adapter *adapter)
 	return 0;
 }
 
-static int e1000_setup_loopback_test(struct e1000_adapter *adapter)
+int e1000_loopback_setup(struct e1000_adapter *adapter)
 {
 	struct e1000_hw *hw = &adapter->hw;
 	u32 rctl;

@@ -1438,7 +1438,7 @@ static int e1000_setup_loopback_test(struct e1000_adapter *adapter)
 	return 7;
 }
 
-static void e1000_loopback_cleanup(struct e1000_adapter *adapter)
+void e1000_loopback_cleanup(struct e1000_adapter *adapter)
 {
 	struct e1000_hw *hw = &adapter->hw;
 	u32 rctl;

@@ -1593,7 +1593,7 @@ static int e1000_loopback_test(struct e1000_adapter *adapter, u64 *data)
 	if (*data)
 		goto out;
 
-	*data = e1000_setup_loopback_test(adapter);
+	*data = e1000_loopback_setup(adapter);
 	if (*data)
 		goto err_loopback;
 

index a83a108..ca45a12 100644
@@ -3256,6 +3256,14 @@ static void e1000_configure(struct e1000_adapter *adapter)
 	e1000_configure_rx(adapter);
 	adapter->alloc_rx_buf(adapter, e1000_desc_unused(adapter->rx_ring),
 			      GFP_KERNEL);
+
+	if (adapter->netdev->features & NETIF_F_LOOPBACK) {
+		if (e1000_loopback_setup(adapter)) {
+			e_warn("Could not activate loopback mode\n");
+			adapter->netdev->features &= ~NETIF_F_LOOPBACK;
+		} else
+			e_info("Loopback mode activated\n");
+	}
 }
 
 /**

@@ -5910,7 +5918,7 @@ static int e1000_set_features(struct net_device *netdev,
 		adapter->flags |= FLAG_TSO_FORCE;
 
 	if (!(changed & (NETIF_F_HW_VLAN_RX | NETIF_F_HW_VLAN_TX |
-			 NETIF_F_RXCSUM)))
+			 NETIF_F_RXCSUM | NETIF_F_LOOPBACK)))
 		return 0;
 
 	/* We will make sure to return 1 below so that we can change

@@ -5919,6 +5927,14 @@ static int e1000_set_features(struct net_device *netdev,
 	 * overriding it. */
 	netdev->features = features;
 
+	/* disable loopback if requested */
+	if ((changed & NETIF_F_LOOPBACK) && (!(features & NETIF_F_LOOPBACK))) {
+		netdev_dbg(netdev, "Disabling loopback mode\n");
+		e1000_loopback_cleanup(adapter);
+	}
+	/* otherwise loopback is enabled via e1000e_reinit_locked
+	 * below and each time the device is up'ed */
+
 	if (netif_running(netdev))
 		e1000e_reinit_locked(adapter);
 	else

@@ -6116,6 +6132,9 @@ static int __devinit e1000_probe(struct pci_dev *pdev,
 	/* Set user-changeable features (subset of all device features) */
 	netdev->hw_features = netdev->features;
 
+	/* Add capabilities, disabled by default */
+	netdev->hw_features |= NETIF_F_LOOPBACK;
+
 	if (adapter->flags & FLAG_HAS_HW_VLAN_FILTER)
 		netdev->features |= NETIF_F_HW_VLAN_FILTER;
 

index 8666476..f487a7f 100644
@@ -1775,39 +1775,53 @@ static s32 e1000_wait_autoneg(struct e1000_hw *hw)
  *  Polls the PHY status register for link, 'iterations' number of times.
  **/
 s32 e1000e_phy_has_link_generic(struct e1000_hw *hw, u32 iterations,
-			       u32 usec_interval, bool *success)
+				u32 usec_interval, bool *success)
 {
-	s32 ret_val = 0;
-	u16 i, phy_status;
+	u32 i;
+	u16 phy_reg;
+	int good_reads_phy_status = 0;
+
+	}
+
+	/*
+	 * Remember that e1e_rphy may fail because of another entity
+	 * (like the firmware) holding the lock, we need to handle
+	 * this gracefully - by waiting and trying again.
+	 *
+	 * Some PHYs require the PHY_STATUS register to be read twice
+	 * due to the link bit being sticky.  No harm doing it across
+	 * the board.
+	 */
+	for (i = 0; i < iterations; /* i incremented manually */) {
+		if (0 == e1e_rphy(hw, PHY_STATUS, &phy_reg)) {
+			if (++good_reads_phy_status < 2)
+				continue; /* Re-read once, to make sure */
 
-	for (i = 0; i < iterations; i++) {
-		/*
-		 * Some PHYs require the PHY_STATUS register to be read
-		 * twice due to the link bit being sticky.  No harm doing
-		 * it across the board.
-		 */
-		ret_val = e1e_rphy(hw, PHY_STATUS, &phy_status);
-		if (ret_val)
 			/*
-			 * If the first read fails, another entity may have
-			 * ownership of the resources, wait and try again to
-			 * see if they have relinquished the resources yet.
+			 * Great, we got at least 2 successfull reads
 			 */
-			udelay(usec_interval);
-		ret_val = e1e_rphy(hw, PHY_STATUS, &phy_status);
-		if (ret_val)
-			break;
-		if (phy_status & MII_SR_LINK_STATUS)
-			break;
-		if (usec_interval >= 1000)
-			mdelay(usec_interval/1000);
+
+			if (phy_reg & MII_SR_LINK_STATUS) {
+				/* success: link up */
+				*success = true;
+				return 0;
+			}
+		}
+
+		/* Pause now and re-iterate */
+		if (++i >= iterations)
+			break; /* Pause not needed after last iteration */
+		else if (usec_interval >= 1000)
+			mdelay(usec_interval / 1000);
 		else
 			udelay(usec_interval);
 	}
 
-	*success = (i < iterations);
-
-	return ret_val;
+	/* no success in for(;;) loop */
+	*success = false;
+	if (good_reads_phy_status < 2)
+		return -E1000_ERR_PHY;
+	return 0;
 }
 
 /**

index f487a7f..9476c8f 100644
@@ -1781,6 +1781,11 @@ s32 e1000e_phy_has_link_generic(struct e1000_hw *hw, u32 iterations,
 	u16 phy_reg;
 	int good_reads_phy_status = 0;
 
+	/* If loopback is enabled, we claim the link is up */
+	if ((0 == e1e_rphy(hw, PHY_CONTROL, &phy_reg))
+	    && (phy_reg & MII_CR_LOOPBACK)) {
+		*success = true;
+		return 0;
 	}
 
 	/*