From: Sven Eckelmann <sven@narfation.org>
Date: Tue, 26 Apr 2016 16:14:47 +0200
Subject: [RFC 7/6] ag71xx: Test link on QCA955x for PHY connectivity problems

The link between MAC and PHY on a QCA955x tends to break down after carrier
changes. This can be worked around by setting the PHY (AR803x only
supported) into digital loopback mode and then sending packets via this
link. If no data is returned then the TX_INVERT bit has to be toggled to
get the link working again.

No information was received from QCA what actually is broken.
---
 .../linux/ar71xx/files/arch/mips/ath79/dev-eth.c   |  44 +++-
 .../ar71xx/files/arch/mips/ath79/mach-mr1750.c     |   1 +
 .../ar71xx/files/arch/mips/ath79/mach-om5pac.c     |   2 +
 .../ar71xx/files/arch/mips/ath79/mach-om5pacv2.c   |   2 +
 .../mips/include/asm/mach-ath79/ag71xx_platform.h  |   2 +
 .../drivers/net/ethernet/atheros/ag71xx/ag71xx.h   |  13 +
 .../net/ethernet/atheros/ag71xx/ag71xx_main.c      | 283 +++++++++++++++++++++
 .../net/ethernet/atheros/ag71xx/ag71xx_phy.c       |  17 +-
 .../999-dont-set-down-on-loopback.patch            |  30 +++
 .../999-dont-set-down-on-loopback.patch            |  30 +++
 .../999-dont-set-down-on-loopback.patch            |  30 +++
 11 files changed, 451 insertions(+), 3 deletions(-)
 create mode 100644 target/linux/generic/patches-3.18/999-dont-set-down-on-loopback.patch
 create mode 100644 target/linux/generic/patches-4.1/999-dont-set-down-on-loopback.patch
 create mode 100644 target/linux/generic/patches-4.4/999-dont-set-down-on-loopback.patch

diff --git a/target/linux/ar71xx/files/arch/mips/ath79/dev-eth.c b/target/linux/ar71xx/files/arch/mips/ath79/dev-eth.c
index b43c80a..d1d3be7 100644
--- a/target/linux/ar71xx/files/arch/mips/ath79/dev-eth.c
+++ b/target/linux/ar71xx/files/arch/mips/ath79/dev-eth.c
@@ -373,10 +373,50 @@ static void ar934x_set_speed_ge0(int speed)
 	iounmap(base);
 }
 
+static u32 qca955x_get_eth_pll(unsigned int mac, int speed)
+{
+	struct ag71xx_platform_data *pdata;
+	struct ath79_eth_pll_data *pll_data;
+	u32 pll_val;
+	u32 tx_invert = 0;
+
+	switch (mac) {
+	case 0:
+		pll_data = &ath79_eth0_pll_data;
+		pdata = &ath79_eth0_data;
+		break;
+	case 1:
+		pll_data = &ath79_eth1_pll_data;
+		pdata = &ath79_eth1_data;
+		break;
+	default:
+		BUG();
+	}
+
+	switch (speed) {
+	case SPEED_10:
+		pll_val = pll_data->pll_10;
+		break;
+	case SPEED_100:
+		pll_val = pll_data->pll_100;
+		break;
+	case SPEED_1000:
+		pll_val = pll_data->pll_1000;
+		break;
+	default:
+		BUG();
+	}
+
+	/* toggle TX_INVERT when required by ag71xx */
+	if (pdata->tx_invert_war && pdata->tx_invert_active)
+		tx_invert =  BIT(31);
+
+	return pll_val ^ tx_invert;
+}
 static void qca955x_set_speed_xmii(int speed)
 {
 	void __iomem *base;
-	u32 val = ath79_get_eth_pll(0, speed);
+	u32 val = qca955x_get_eth_pll(0, speed);
 
 	base = ioremap_nocache(AR71XX_PLL_BASE, AR71XX_PLL_SIZE);
 	__raw_writel(val, base + QCA955X_PLL_ETH_XMII_CONTROL_REG);
@@ -386,7 +426,7 @@ static void qca955x_set_speed_xmii(int speed)
 static void qca955x_set_speed_sgmii(int speed)
 {
 	void __iomem *base;
-	u32 val = ath79_get_eth_pll(1, speed);
+	u32 val = qca955x_get_eth_pll(1, speed);
 
 	base = ioremap_nocache(AR71XX_PLL_BASE, AR71XX_PLL_SIZE);
 	__raw_writel(val, base + QCA955X_PLL_ETH_SGMII_CONTROL_REG);
diff --git a/target/linux/ar71xx/files/arch/mips/ath79/mach-mr1750.c b/target/linux/ar71xx/files/arch/mips/ath79/mach-mr1750.c
index e3c04e7..97b5bf4 100644
--- a/target/linux/ar71xx/files/arch/mips/ath79/mach-mr1750.c
+++ b/target/linux/ar71xx/files/arch/mips/ath79/mach-mr1750.c
@@ -162,6 +162,7 @@ static void __init mr1750_setup(void)
 	/* GMAC0 is connected to the RMGII interface */
 	ath79_eth0_data.phy_if_mode = PHY_INTERFACE_MODE_RGMII;
 	ath79_eth0_data.phy_mask = BIT(5);
+	ath79_eth0_data.tx_invert_war = 1;
 	ath79_eth0_data.mii_bus_dev = &ath79_mdio0_device.dev;
 
 	ath79_register_eth(0);
diff --git a/target/linux/ar71xx/files/arch/mips/ath79/mach-om5pac.c b/target/linux/ar71xx/files/arch/mips/ath79/mach-om5pac.c
index f6974af..9387f26 100644
--- a/target/linux/ar71xx/files/arch/mips/ath79/mach-om5pac.c
+++ b/target/linux/ar71xx/files/arch/mips/ath79/mach-om5pac.c
@@ -171,6 +171,7 @@ static void __init om5p_ac_setup(void)
 	ath79_eth0_data.phy_if_mode = PHY_INTERFACE_MODE_RGMII;
 	ath79_eth0_data.mii_bus_dev = &ath79_mdio0_device.dev;
 	ath79_eth0_data.phy_mask = BIT(1);
+	ath79_eth0_data.tx_invert_war = 1;
 	ath79_eth0_pll_data.pll_1000 = 0x82000101;
 	ath79_eth0_pll_data.pll_100 = 0x80000101;
 	ath79_eth0_pll_data.pll_10 = 0x80001313;
@@ -180,6 +181,7 @@ static void __init om5p_ac_setup(void)
 	ath79_eth1_data.phy_if_mode = PHY_INTERFACE_MODE_SGMII;
 	ath79_eth1_data.mii_bus_dev = &ath79_mdio0_device.dev;
 	ath79_eth1_data.phy_mask = BIT(2);
+	ath79_eth1_data.tx_invert_war = 1;
 	ath79_eth1_pll_data.pll_1000 = 0x03000101;
 	ath79_eth1_pll_data.pll_100 = 0x80000101;
 	ath79_eth1_pll_data.pll_10 = 0x80001313;
diff --git a/target/linux/ar71xx/files/arch/mips/ath79/mach-om5pacv2.c b/target/linux/ar71xx/files/arch/mips/ath79/mach-om5pacv2.c
index 0480d01..820a10a 100644
--- a/target/linux/ar71xx/files/arch/mips/ath79/mach-om5pacv2.c
+++ b/target/linux/ar71xx/files/arch/mips/ath79/mach-om5pacv2.c
@@ -199,6 +199,7 @@ static void __init om5p_acv2_setup(void)
 	ath79_eth0_data.phy_if_mode = PHY_INTERFACE_MODE_RGMII;
 	ath79_eth0_data.mii_bus_dev = &ath79_mdio0_device.dev;
 	ath79_eth0_data.phy_mask = BIT(4);
+	ath79_eth0_data.tx_invert_war = 1;
 	ath79_eth0_pll_data.pll_1000 = 0x82000101;
 	ath79_eth0_pll_data.pll_100 = 0x80000101;
 	ath79_eth0_pll_data.pll_10 = 0x80001313;
@@ -208,6 +209,7 @@ static void __init om5p_acv2_setup(void)
 	ath79_eth1_data.phy_if_mode = PHY_INTERFACE_MODE_SGMII;
 	ath79_eth1_data.mii_bus_dev = &ath79_mdio1_device.dev;
 	ath79_eth1_data.phy_mask = BIT(1);
+	ath79_eth1_data.tx_invert_war = 0;
 	ath79_eth1_pll_data.pll_1000 = 0x03000101;
 	ath79_eth1_pll_data.pll_100 = 0x80000101;
 	ath79_eth1_pll_data.pll_10 = 0x80001313;
diff --git a/target/linux/ar71xx/files/arch/mips/include/asm/mach-ath79/ag71xx_platform.h b/target/linux/ar71xx/files/arch/mips/include/asm/mach-ath79/ag71xx_platform.h
index d46dc4e..33575ed 100644
--- a/target/linux/ar71xx/files/arch/mips/include/asm/mach-ath79/ag71xx_platform.h
+++ b/target/linux/ar71xx/files/arch/mips/include/asm/mach-ath79/ag71xx_platform.h
@@ -31,6 +31,8 @@ struct ag71xx_platform_data {
 	u8		mac_addr[ETH_ALEN];
 	struct device	*mii_bus_dev;
 
+	u8		tx_invert_war:1;
+	u8		tx_invert_active:1;
 	u8		has_gbit:1;
 	u8		is_ar91xx:1;
 	u8		is_ar7240:1;
diff --git a/target/linux/ar71xx/files/drivers/net/ethernet/atheros/ag71xx/ag71xx.h b/target/linux/ar71xx/files/drivers/net/ethernet/atheros/ag71xx/ag71xx.h
index 5d03dcf..b6b7794 100644
--- a/target/linux/ar71xx/files/drivers/net/ethernet/atheros/ag71xx/ag71xx.h
+++ b/target/linux/ar71xx/files/drivers/net/ethernet/atheros/ag71xx/ag71xx.h
@@ -147,6 +147,13 @@ struct ag71xx_debug {
 	struct ag71xx_napi_stats napi_stats;
 };
 
+enum ag71xx_lpb_test {
+	AG71XX_LPB_TEST_DISABLED,
+	AG71XX_LPB_TEST_ENABLED,
+	AG71XX_LPB_TEST_SUCCESS,
+	AG71XX_LPB_TEST_POST_LINKRESET,
+};
+
 struct ag71xx {
 	void __iomem		*mac_base;
 
@@ -174,8 +181,11 @@ struct ag71xx {
 	unsigned int		desc_pktlen_mask;
 	unsigned int		rx_buf_size;
 
+	enum ag71xx_lpb_test	loopback;
+
 	struct work_struct	restart_work;
 	struct delayed_work	link_work;
+	struct work_struct	tx_invert_work;
 	struct timer_list	oom_timer;
 
 #ifdef CONFIG_AG71XX_DEBUG_FS
@@ -477,6 +487,9 @@ static inline void ag71xx_debugfs_update_napi_stats(struct ag71xx *ag,
 						    int rx, int tx) {}
 #endif /* CONFIG_AG71XX_DEBUG_FS */
 
+void enable_loopback_selftest(struct ag71xx *ag);
+void disable_loopback_selftest(struct ag71xx *ag);
+
 void ag71xx_ar7240_start(struct ag71xx *ag);
 void ag71xx_ar7240_stop(struct ag71xx *ag);
 int ag71xx_ar7240_init(struct ag71xx *ag);
diff --git a/target/linux/ar71xx/files/drivers/net/ethernet/atheros/ag71xx/ag71xx_main.c b/target/linux/ar71xx/files/drivers/net/ethernet/atheros/ag71xx/ag71xx_main.c
index 38226cf..6d17cbd 100644
--- a/target/linux/ar71xx/files/drivers/net/ethernet/atheros/ag71xx/ag71xx_main.c
+++ b/target/linux/ar71xx/files/drivers/net/ethernet/atheros/ag71xx/ag71xx_main.c
@@ -920,6 +920,197 @@ static void ag71xx_restart_work_func(struct work_struct *work)
 	rtnl_unlock();
 }
 
+static void ag71xx_set_digital_loopback(struct ag71xx *ag, bool enable,
+					unsigned int speed)
+{
+	struct phy_device *phydev = ag->phy_dev;
+	struct ag71xx_platform_data *pdata = ag71xx_get_pdata(ag);
+	u32 val;
+	u32 speed_val;
+	u32 linktests = 100;
+
+	netif_carrier_off(ag->dev);
+	netif_stop_queue(ag->dev);
+
+	/* this is bad because it will cause even more link changes
+	 * but this is all we got from QCA
+	 */
+	if (enable) {
+		switch (speed) {
+		case SPEED_1000:
+			speed_val = 0x4140;
+			break;
+		case SPEED_100:
+			speed_val = 0x6100;
+			break;
+		case SPEED_10:
+			speed_val = 0x4100;
+			break;
+		}
+	} else {
+		/* autoneg */
+		speed_val = 0x1000;
+	}
+
+	phy_write(phydev, MII_BMCR, speed_val);
+	if (enable) {
+		do {
+			udelay(10);
+			val = phy_read(phydev, 0x11);
+		} while (linktests-- > 0 && !(val & 0x0400));
+
+		ag->link = 1;
+		ag->duplex = DUPLEX_FULL; /* force full duplex for test */
+		ag->speed = speed;
+		ag71xx_link_adjust(ag);
+	}
+}
+
+static int send_looback_packets(struct ag71xx *ag)
+{
+	unsigned int i;
+	struct sk_buff *skb;
+	struct ethhdr *ethhdr;
+	size_t skb_len = 64;
+	size_t payload_len = skb_len - sizeof(*ethhdr);
+	u8 *payload;
+	int ret;
+
+	for (i = 0; i < 10; i++) {
+		skb = dev_alloc_skb(skb_len);
+		if (!skb)
+			return -ENOMEM;
+
+		ethhdr = (struct ethhdr *)skb_put(skb, sizeof(*ethhdr));
+		ether_addr_copy(ethhdr->h_source, ag->dev->dev_addr);
+		memset(ethhdr->h_dest, 0xff, sizeof(ethhdr->h_dest));
+		ethhdr->h_proto = htons(payload_len);
+
+		payload = skb_put(skb, payload_len);
+		memset(payload, 0x1, payload_len);
+
+		ret = ag71xx_hard_start_xmit(skb, ag->dev);
+		if (ret != NETDEV_TX_OK)
+			return -EIO;
+	}
+
+	return 0;
+}
+
+static bool loopback_packet_check(struct ag71xx *ag, struct sk_buff *skb,
+				  int pktlen)
+{
+	struct ethhdr *ethhdr;
+	size_t skb_len = 64;
+	size_t payload_len = skb_len - sizeof(*ethhdr);
+	u8 *payload;
+	unsigned int i;
+
+	if (pktlen < skb_len)
+		return false;
+
+	ethhdr = (struct ethhdr *)skb->data;
+	if (!ether_addr_equal(ethhdr->h_source, ag->dev->dev_addr))
+		return false;
+
+	if (!is_broadcast_ether_addr(ethhdr->h_dest))
+		return false;
+
+	if (ethhdr->h_proto != htons(payload_len))
+		return false;
+
+	payload = &skb->data[sizeof(*ethhdr)];
+	for (i = 0; i < payload_len; i++) {
+		if (payload[i] != 0x1)
+			return false;
+	}
+
+	return true;
+}
+
+static int ag71xx_rx_packets(struct ag71xx *ag, int limit);
+
+static void ag71xx_tx_invert_work_func(struct work_struct *work)
+{
+	unsigned long flags;
+	struct ag71xx *ag = container_of(work, struct ag71xx, tx_invert_work);
+	struct ag71xx_platform_data *pdata = ag71xx_get_pdata(ag);
+	bool require_link_adjust = false;
+	int ret;
+	int rx_done;
+	int testruns;
+
+	pr_info("start loopback test\n");
+
+	testruns = 5;
+	do {
+		ret = send_looback_packets(ag);
+		if (ret < 0) {
+			pr_err("Failed to send loopback packets\n");
+			goto out;
+		}
+
+		rx_done = ag71xx_rx_packets(ag, 10);
+		mdelay(10);
+	} while (testruns-- > 0 && ag->loopback != AG71XX_LPB_TEST_SUCCESS);
+
+	/* test is enabled but was not successful */
+	spin_lock_irqsave(&ag->lock, flags);
+	if (ag->loopback == AG71XX_LPB_TEST_ENABLED) {
+		/* have to toggle tx_invert but first test if it works */
+		pdata->tx_invert_active = !pdata->tx_invert_active;
+		require_link_adjust = true;
+	}
+	spin_unlock_irqrestore(&ag->lock, flags);
+	
+	if (require_link_adjust) {
+		ag71xx_link_adjust(ag);
+		require_link_adjust = false;
+	} else {
+		goto finish;
+	}
+
+	/* RETEST */
+	testruns = 5;
+	do {
+		ret = send_looback_packets(ag);
+		if (ret < 0) {
+			pr_err("Failed to send loopback packets\n");
+			goto out;
+		}
+
+		rx_done = ag71xx_rx_packets(ag, 10);
+		mdelay(10);
+	} while (testruns-- > 0 && ag->loopback != AG71XX_LPB_TEST_SUCCESS);
+
+	/* test is enabled but was not successful */
+	spin_lock_irqsave(&ag->lock, flags);
+	if (ag->loopback == AG71XX_LPB_TEST_ENABLED) {
+		pr_err("no valid TX_INVERT setting found in loopback test\n");
+		pdata->tx_invert_active = !pdata->tx_invert_active;
+		require_link_adjust = true;
+	}
+	spin_unlock_irqrestore(&ag->lock, flags);
+
+	if (require_link_adjust) {
+		ag71xx_link_adjust(ag);
+		require_link_adjust = false;
+	} else {
+		goto finish;
+	}
+
+finish:
+	/* damn, the hardware often hangs after the link changes - kick it */
+	schedule_work(&ag->restart_work);
+
+out:
+	/* disable loopback test again */
+	ag71xx_set_digital_loopback(ag, false, 0);
+	spin_lock_irqsave(&ag->lock, flags);
+	ag->loopback = AG71XX_LPB_TEST_POST_LINKRESET;
+	spin_unlock_irqrestore(&ag->lock, flags);
+}
+
 static bool ag71xx_check_dma_stuck(struct ag71xx *ag, unsigned long timestamp)
 {
 	u32 rx_sm, tx_sm, rx_fd;
@@ -1014,10 +1205,21 @@ static int ag71xx_rx_packets(struct ag71xx *ag, int limit)
 	struct sk_buff_head queue;
 	struct sk_buff *skb;
 	int done = 0;
+	bool testloopback;
+	bool loopback_found = false;
+	unsigned long flags;
 
 	DBG("%s: rx packets, limit=%d, curr=%u, dirty=%u\n",
 			dev->name, limit, ring->curr, ring->dirty);
 
+	/* check if loopback test is currently started */
+	spin_lock_irqsave(&ag->lock, flags);
+	if (ag->loopback == AG71XX_LPB_TEST_ENABLED)
+		testloopback = true;
+	else
+		testloopback = false;
+	spin_unlock_irqrestore(&ag->lock, flags);
+
 	skb_queue_head_init(&queue);
 
 	while (done < limit) {
@@ -1057,6 +1259,14 @@ static int ag71xx_rx_packets(struct ag71xx *ag, int limit)
 		if (ag71xx_has_ar8216(ag))
 			err = ag71xx_remove_ar8216_header(ag, skb, pktlen);
 
+		if (!err && testloopback &&
+		    loopback_packet_check(ag, skb, pktlen)) {
+			loopback_found = true;
+
+			/* drop this packet below */
+			err = 1;
+		}
+
 		if (err) {
 			dev->stats.rx_dropped++;
 			kfree_skb(skb);
@@ -1083,6 +1293,14 @@ next:
 	DBG("%s: rx finish, curr=%u, dirty=%u, done=%d\n",
 		dev->name, ring->curr, ring->dirty, done);
 
+	/* mark loopback test as successful */
+	if (loopback_found) {
+		spin_lock_irqsave(&ag->lock, flags);
+		if (ag->loopback == AG71XX_LPB_TEST_ENABLED)
+			ag->loopback = AG71XX_LPB_TEST_SUCCESS;
+		spin_unlock_irqrestore(&ag->lock, flags);
+	}
+
 	return done;
 }
 
@@ -1099,6 +1317,16 @@ static int ag71xx_poll(struct napi_struct *napi, int limit)
 	int rx_done;
 
 	pdata->ddr_flush();
+
+	/* receive via loopback code */
+	spin_lock_irqsave(&ag->lock, flags);
+	if (ag->loopback == AG71XX_LPB_TEST_ENABLED) {
+		spin_unlock_irqrestore(&ag->lock, flags);
+		napi_complete(napi);
+		return 0;
+	}
+	spin_unlock_irqrestore(&ag->lock, flags);
+
 	tx_done = ag71xx_tx_packets(ag, false);
 
 	DBG("%s: processing RX ring\n", dev->name);
@@ -1250,6 +1478,59 @@ static const char *ag71xx_get_phy_if_mode_name(phy_interface_t mode)
 	return "unknown";
 }
 
+void enable_loopback_selftest(struct ag71xx *ag)
+{
+	unsigned long flags;
+	unsigned int speed;
+	struct ag71xx_platform_data *pdata = ag71xx_get_pdata(ag);
+
+	if (!pdata->tx_invert_war)
+		return;
+
+	spin_lock_irqsave(&ag->lock, flags);
+	/* check for valid speeds */
+	switch (ag->speed) {
+	case SPEED_1000:
+	case SPEED_100:
+	case SPEED_10:
+		speed = ag->speed;
+		break;
+	default:
+		spin_unlock_irqrestore(&ag->lock, flags);
+		return;
+	}
+
+	if (ag->loopback != AG71XX_LPB_TEST_DISABLED) {
+		spin_unlock_irqrestore(&ag->lock, flags);
+		return;
+	}
+	ag->loopback = AG71XX_LPB_TEST_ENABLED;
+	spin_unlock_irqrestore(&ag->lock, flags);
+
+	ag71xx_set_digital_loopback(ag, true, speed);
+	schedule_work(&ag->tx_invert_work);
+}
+
+void disable_loopback_selftest(struct ag71xx *ag)
+{
+	unsigned long flags;
+	struct ag71xx_platform_data *pdata = ag71xx_get_pdata(ag);
+
+	if (!pdata->tx_invert_war)
+		return;
+
+	spin_lock_irqsave(&ag->lock, flags);
+	if (ag->loopback == AG71XX_LPB_TEST_DISABLED) {
+		spin_unlock_irqrestore(&ag->lock, flags);
+		return;
+	}
+	ag->loopback = AG71XX_LPB_TEST_DISABLED;
+	spin_unlock_irqrestore(&ag->lock, flags);
+
+	ag71xx_set_digital_loopback(ag, false, 0);
+	cancel_work_sync(&ag->tx_invert_work);
+}
+
 
 static int ag71xx_probe(struct platform_device *pdev)
 {
@@ -1319,6 +1600,7 @@ static int ag71xx_probe(struct platform_device *pdev)
 	dev->ethtool_ops = &ag71xx_ethtool_ops;
 
 	INIT_WORK(&ag->restart_work, ag71xx_restart_work_func);
+	INIT_WORK(&ag->tx_invert_work, ag71xx_tx_invert_work_func);
 
 	init_timer(&ag->oom_timer);
 	ag->oom_timer.data = (unsigned long) dev;
@@ -1403,6 +1685,7 @@ static int ag71xx_remove(struct platform_device *pdev)
 	if (dev) {
 		struct ag71xx *ag = netdev_priv(dev);
 
+		disable_loopback_selftest(ag);
 		ag71xx_debugfs_exit(ag);
 		ag71xx_phy_disconnect(ag);
 		unregister_netdev(dev);
diff --git a/target/linux/ar71xx/files/drivers/net/ethernet/atheros/ag71xx/ag71xx_phy.c b/target/linux/ar71xx/files/drivers/net/ethernet/atheros/ag71xx/ag71xx_phy.c
index 9de77e9..0eae954 100644
--- a/target/linux/ar71xx/files/drivers/net/ethernet/atheros/ag71xx/ag71xx_phy.c
+++ b/target/linux/ar71xx/files/drivers/net/ethernet/atheros/ag71xx/ag71xx_phy.c
@@ -22,6 +22,11 @@ static void ag71xx_phy_link_adjust(struct net_device *dev)
 
 	spin_lock_irqsave(&ag->lock, flags);
 
+	if (ag->loopback != AG71XX_LPB_TEST_DISABLED && 
+	    ag->loopback != AG71XX_LPB_TEST_POST_LINKRESET) {
+		goto out;
+	}
+
 	if (phydev->link) {
 		if (ag->duplex != phydev->duplex
 		    || ag->speed != phydev->speed) {
@@ -36,9 +41,19 @@ static void ag71xx_phy_link_adjust(struct net_device *dev)
 	ag->duplex = phydev->duplex;
 	ag->speed = phydev->speed;
 
-	if (status_change)
+	if (status_change) {
 		ag71xx_link_adjust(ag);
+		
+		if (!phydev->link)
+			goto out;
 
+		if (ag->loopback == AG71XX_LPB_TEST_POST_LINKRESET)
+			ag->loopback = AG71XX_LPB_TEST_DISABLED;
+		else
+			enable_loopback_selftest(ag);
+	}
+
+out:
 	spin_unlock_irqrestore(&ag->lock, flags);
 }
 
diff --git a/target/linux/generic/patches-3.18/999-dont-set-down-on-loopback.patch b/target/linux/generic/patches-3.18/999-dont-set-down-on-loopback.patch
new file mode 100644
index 0000000..eda560a
--- /dev/null
+++ b/target/linux/generic/patches-3.18/999-dont-set-down-on-loopback.patch
@@ -0,0 +1,30 @@
+--- a/drivers/net/phy/phy_device.c
++++ b/drivers/net/phy/phy_device.c
+@@ -916,6 +916,8 @@ static int gen10g_config_aneg(struct phy
+ int genphy_update_link(struct phy_device *phydev)
+ {
+ 	int status;
++	u32 loopback;
++	u32 bmcr;
+ 
+ 	if (phydev->drv->update_link)
+ 		return phydev->drv->update_link(phydev);
+@@ -924,13 +926,17 @@ int genphy_update_link(struct phy_device
+ 	status = phy_read(phydev, MII_BMSR);
+ 	if (status < 0)
+ 		return status;
++	/* HACK: check for loopback mode */
++	bmcr = phy_read(phydev, MII_BMCR);
++	bmcr = phy_read(phydev, MII_BMCR);
++	loopback = bmcr & BMCR_LOOPBACK;
+ 
+ 	/* Read link and autonegotiation status */
+ 	status = phy_read(phydev, MII_BMSR);
+ 	if (status < 0)
+ 		return status;
+ 
+-	if ((status & BMSR_LSTATUS) == 0)
++	if ((status & BMSR_LSTATUS) == 0 && !loopback)
+ 		phydev->link = 0;
+ 	else
+ 		phydev->link = 1;
diff --git a/target/linux/generic/patches-4.1/999-dont-set-down-on-loopback.patch b/target/linux/generic/patches-4.1/999-dont-set-down-on-loopback.patch
new file mode 100644
index 0000000..eda560a
--- /dev/null
+++ b/target/linux/generic/patches-4.1/999-dont-set-down-on-loopback.patch
@@ -0,0 +1,30 @@
+--- a/drivers/net/phy/phy_device.c
++++ b/drivers/net/phy/phy_device.c
+@@ -916,6 +916,8 @@ static int gen10g_config_aneg(struct phy
+ int genphy_update_link(struct phy_device *phydev)
+ {
+ 	int status;
++	u32 loopback;
++	u32 bmcr;
+ 
+ 	if (phydev->drv->update_link)
+ 		return phydev->drv->update_link(phydev);
+@@ -924,13 +926,17 @@ int genphy_update_link(struct phy_device
+ 	status = phy_read(phydev, MII_BMSR);
+ 	if (status < 0)
+ 		return status;
++	/* HACK: check for loopback mode */
++	bmcr = phy_read(phydev, MII_BMCR);
++	bmcr = phy_read(phydev, MII_BMCR);
++	loopback = bmcr & BMCR_LOOPBACK;
+ 
+ 	/* Read link and autonegotiation status */
+ 	status = phy_read(phydev, MII_BMSR);
+ 	if (status < 0)
+ 		return status;
+ 
+-	if ((status & BMSR_LSTATUS) == 0)
++	if ((status & BMSR_LSTATUS) == 0 && !loopback)
+ 		phydev->link = 0;
+ 	else
+ 		phydev->link = 1;
diff --git a/target/linux/generic/patches-4.4/999-dont-set-down-on-loopback.patch b/target/linux/generic/patches-4.4/999-dont-set-down-on-loopback.patch
new file mode 100644
index 0000000..eda560a
--- /dev/null
+++ b/target/linux/generic/patches-4.4/999-dont-set-down-on-loopback.patch
@@ -0,0 +1,30 @@
+--- a/drivers/net/phy/phy_device.c
++++ b/drivers/net/phy/phy_device.c
+@@ -916,6 +916,8 @@ static int gen10g_config_aneg(struct phy
+ int genphy_update_link(struct phy_device *phydev)
+ {
+ 	int status;
++	u32 loopback;
++	u32 bmcr;
+ 
+ 	if (phydev->drv->update_link)
+ 		return phydev->drv->update_link(phydev);
+@@ -924,13 +926,17 @@ int genphy_update_link(struct phy_device
+ 	status = phy_read(phydev, MII_BMSR);
+ 	if (status < 0)
+ 		return status;
++	/* HACK: check for loopback mode */
++	bmcr = phy_read(phydev, MII_BMCR);
++	bmcr = phy_read(phydev, MII_BMCR);
++	loopback = bmcr & BMCR_LOOPBACK;
+ 
+ 	/* Read link and autonegotiation status */
+ 	status = phy_read(phydev, MII_BMSR);
+ 	if (status < 0)
+ 		return status;
+ 
+-	if ((status & BMSR_LSTATUS) == 0)
++	if ((status & BMSR_LSTATUS) == 0 && !loopback)
+ 		phydev->link = 0;
+ 	else
+ 		phydev->link = 1;
