From 0dc9de2efd7ea6fb613afec822b52b4bfe5a7b2e Mon Sep 17 00:00:00 2001
From: Sascha Hauer <s.hauer@pengutronix.de>
Date: Wed, 26 Sep 2012 17:53:29 +0200
Subject: [PATCH] net/eth: fix link handling

Check link status on eth device open time and then periodically
every 5 seconds.

Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>
---
 drivers/net/phy/phy.c | 35 ++++++++++++++------
 include/linux/phy.h   |  3 ++
 net/eth.c             | 75 ++++++++++++++++++++++++++++++++-----------
 3 files changed, 85 insertions(+), 28 deletions(-)

diff --git a/drivers/net/phy/phy.c b/drivers/net/phy/phy.c
index e1a24faf9..88c3ff772 100644
--- a/drivers/net/phy/phy.c
+++ b/drivers/net/phy/phy.c
@@ -29,6 +29,30 @@
 
 static int genphy_config_init(struct phy_device *phydev);
 
+int phy_update_status(struct phy_device *dev)
+{
+	struct phy_driver *drv = to_phy_driver(dev->dev.driver);
+	struct eth_device *edev = dev->attached_dev;
+	int ret;
+	int oldspeed = dev->speed, oldduplex = dev->duplex;
+
+	ret = drv->read_status(dev);
+	if (ret)
+		return ret;
+
+	if (dev->speed == oldspeed && dev->duplex == oldduplex)
+		return 0;
+
+	if (dev->adjust_link)
+		dev->adjust_link(edev);
+
+	if (dev->link)
+		printf("%dMbps %s duplex link detected\n", dev->speed,
+			dev->duplex ? "full" : "half");
+
+	return 0;
+}
+
 struct phy_device *phy_device_create(struct mii_bus *bus, int addr, int phy_id)
 {
 	struct phy_device *dev;
@@ -172,16 +196,7 @@ int phy_device_connect(struct eth_device *edev, struct mii_bus *bus, int addr,
 
 	drv->config_aneg(dev);
 
-	ret = drv->read_status(dev);
-	if (ret < 0)
-		return ret;
-
-	if (dev->link)
-		printf("%dMbps %s duplex link detected\n", dev->speed,
-			dev->duplex ? "full" : "half");
-
-	if (adjust_link)
-		adjust_link(edev);
+	dev->adjust_link = adjust_link;
 
 	return 0;
 
diff --git a/include/linux/phy.h b/include/linux/phy.h
index 8fe6b866f..76f9edb23 100644
--- a/include/linux/phy.h
+++ b/include/linux/phy.h
@@ -164,6 +164,7 @@ struct phy_device {
 	void *priv;
 
 	struct eth_device *attached_dev;
+	void (*adjust_link)(struct eth_device *dev);
 
 	struct cdev cdev;
 };
@@ -253,6 +254,8 @@ int phy_device_connect(struct eth_device *dev, struct mii_bus *bus, int addr,
 		       void (*adjust_link) (struct eth_device *edev),
 		       u32 flags, phy_interface_t interface);
 
+int phy_update_status(struct phy_device *phydev);
+
 /* Generic PHY support and helper functions */
 int genphy_restart_aneg(struct phy_device *phydev);
 int genphy_config_aneg(struct phy_device *phydev);
diff --git a/net/eth.c b/net/eth.c
index 6cfe4f2bb..9f528709d 100644
--- a/net/eth.c
+++ b/net/eth.c
@@ -32,6 +32,7 @@
 #include <malloc.h>
 
 static struct eth_device *eth_current;
+static uint64_t last_link_check;
 
 static LIST_HEAD(netdev_list);
 
@@ -128,23 +129,63 @@ int eth_complete(struct string_list *sl, char *instr)
 }
 #endif
 
-int eth_send(void *packet, int length)
+/*
+ * Check for link if we haven't done so for longer.
+ */
+static int eth_carrier_check(int force)
+{
+	int ret;
+
+	if (!IS_ENABLED(CONFIG_PHYLIB))
+		return 0;
+
+	if (!eth_current->phydev)
+		return 0;
+
+	if (force || is_timeout(last_link_check, 5 * SECOND)) {
+		ret = phy_update_status(eth_current->phydev);
+		if (ret)
+			return ret;
+		last_link_check = get_time_ns();
+	}
+
+	return eth_current->phydev->link ? 0 : -ENETDOWN;
+}
+
+/*
+ * Check if we have a current ethernet device and
+ * eventually open it if we have to.
+ */
+static int eth_check_open(void)
 {
 	int ret;
 
 	if (!eth_current)
 		return -ENODEV;
 
-	if (!eth_current->active) {
-		ret = eth_current->open(eth_current);
-		if (ret)
-			return ret;
+	if (eth_current->active)
+		return 0;
 
-		if (eth_current->phydev)
-			eth_current->active = eth_current->phydev->link;
-		else
-			eth_current->active = 1;
-	}
+	ret = eth_current->open(eth_current);
+	if (ret)
+		return ret;
+
+	eth_current->active = 1;
+
+	return eth_carrier_check(1);
+}
+
+int eth_send(void *packet, int length)
+{
+	int ret;
+
+	ret = eth_check_open();
+	if (ret)
+		return ret;
+
+	ret = eth_carrier_check(0);
+	if (ret)
+		return ret;
 
 	led_trigger_network(LED_TRIGGER_NET_TX);
 
@@ -155,15 +196,13 @@ int eth_rx(void)
 {
 	int ret;
 
-	if (!eth_current)
-		return -ENODEV;
+	ret = eth_check_open();
+	if (ret)
+		return ret;
 
-	if (!eth_current->active) {
-		ret = eth_current->open(eth_current);
-		if (ret)
-			return ret;
-		eth_current->active = 1;
-	}
+	ret = eth_carrier_check(0);
+	if (ret)
+		return ret;
 
 	return eth_current->recv(eth_current);
 }