From f2134f8e9eb006bdcd729e89f309c07b2fa45180 Mon Sep 17 00:00:00 2001
From: Haavard Skinnemoen <hskinnemoen@atmel.com>
Date: Wed, 2 May 2007 13:31:53 +0200
Subject: [PATCH] macb: Don't restart autonegotiation if we already have link

Rework macb_phy_init so that it doesn't attempt to re-negotiate if the
link is already up.

Signed-off-by: Haavard Skinnemoen <hskinnemoen@atmel.com>
---
 drivers/macb.c | 41 ++++++++++++++++++++++++-----------------
 1 file changed, 24 insertions(+), 17 deletions(-)

diff --git a/drivers/macb.c b/drivers/macb.c
index 43bb878..abe57ba 100644
--- a/drivers/macb.c
+++ b/drivers/macb.c
@@ -292,20 +292,11 @@ static int macb_recv(struct eth_device *netdev)
 	return 0;
 }
 
-static int macb_phy_init(struct macb_device *macb)
+static void macb_phy_reset(struct macb_device *macb)
 {
 	struct eth_device *netdev = &macb->netdev;
-	u32 ncfgr;
-	u16 phy_id, status, adv, lpa;
-	int media, speed, duplex;
 	int i;
-
-	/* Check if the PHY is up to snuff... */
-	phy_id = macb_mdio_read(macb, MII_PHYSID1);
-	if (phy_id == 0xffff) {
-		printf("%s: No PHY present\n", netdev->name);
-		return 0;
-	}
+	u16 status, adv;
 
 	adv = ADVERTISE_CSMA | ADVERTISE_ALL;
 	macb_mdio_write(macb, MII_ADVERTISE, adv);
@@ -313,11 +304,6 @@ static int macb_phy_init(struct macb_device *macb)
 	macb_mdio_write(macb, MII_BMCR, (BMCR_ANENABLE
 					 | BMCR_ANRESTART));
 
-#if 0
-	for (i = 0; i < 9; i++)
-		printf("mii%d: 0x%04x\n", i, macb_mdio_read(macb, i));
-#endif
-
 	for (i = 0; i < CFG_MACB_AUTONEG_TIMEOUT / 100; i++) {
 		status = macb_mdio_read(macb, MII_BMSR);
 		if (status & BMSR_ANEGCOMPLETE)
@@ -330,13 +316,33 @@ static int macb_phy_init(struct macb_device *macb)
 	else
 		printf("%s: Autonegotiation timed out (status=0x%04x)\n",
 		       netdev->name, status);
+}
+
+static int macb_phy_init(struct macb_device *macb)
+{
+	struct eth_device *netdev = &macb->netdev;
+	u32 ncfgr;
+	u16 phy_id, status, adv, lpa;
+	int media, speed, duplex;
+	int i;
+
+	/* Check if the PHY is up to snuff... */
+	phy_id = macb_mdio_read(macb, MII_PHYSID1);
+	if (phy_id == 0xffff) {
+		printf("%s: No PHY present\n", netdev->name);
+		return 0;
+	}
 
+	status = macb_mdio_read(macb, MII_BMSR);
 	if (!(status & BMSR_LSTATUS)) {
+		/* Try to re-negotiate if we don't have link already. */
+		macb_phy_reset(macb);
+
 		for (i = 0; i < CFG_MACB_AUTONEG_TIMEOUT / 100; i++) {
-			udelay(100);
 			status = macb_mdio_read(macb, MII_BMSR);
 			if (status & BMSR_LSTATUS)
 				break;
+			udelay(100);
 		}
 	}
 
@@ -345,6 +351,7 @@ static int macb_phy_init(struct macb_device *macb)
 		       netdev->name, status);
 		return 0;
 	} else {
+		adv = macb_mdio_read(macb, MII_ADVERTISE);
 		lpa = macb_mdio_read(macb, MII_LPA);
 		media = mii_nway_result(lpa & adv);
 		speed = (media & (ADVERTISE_100FULL | ADVERTISE_100HALF)