[PATCH 3/3] net cpsw: check phy status on send and receive

[chf.fritz at googlemail.com]

From: Christoph Fritz <chf.fritz at googlemail.com>

This patch adds phy status checks on cpsw_send() and cpsw_recv().
This is derived from upstream.

Signed-off-by: Christoph Fritz <chf.fritz at googlemail.com>
---
 drivers/net/cpsw.c |   24 ++++++++++++++++++++++++
 1 file changed, 24 insertions(+)

diff --git a/drivers/net/cpsw.c b/drivers/net/cpsw.c
index ec3263d..9e7b320 100644
--- a/drivers/net/cpsw.c
+++ b/drivers/net/cpsw.c
@@ -229,6 +229,9 @@ struct cpsw_priv {
 	struct cpdma_chan		rx_chan, tx_chan;
 
 	struct cpsw_slave		*slaves;
+
+	u32				mdio_link;
+	u32				phy_mask;
 };
 
 static int cpsw_ale_get_field(u32 *ale_entry, u32 start, u32 bits)
@@ -608,14 +611,28 @@ static void cpsw_slave_update_link(struct cpsw_slave *slave,
 static int cpsw_update_link(struct cpsw_slave *slave, struct cpsw_priv *priv)
 {
 	int link = 0;
+	struct cpsw_mdio_regs *mdio_regs = priv->mdio_regs;
 
 	dev_dbg(priv->dev, "* %s\n", __func__);
 
 	cpsw_slave_update_link(slave, priv, &link);
 
+	priv->mdio_link = readl(&mdio_regs->link);
 	return link;
 }
 
+static int cpsw_check_link(struct cpsw_priv *priv)
+{
+	u32 link = 0;
+	struct cpsw_mdio_regs *mdio_regs = priv->mdio_regs;
+
+	link = readl(&mdio_regs->link) & priv->phy_mask;
+	if ((link) && (link == priv->mdio_link))
+		return 1;
+
+	return cpsw_update_link(&priv->slaves[0], priv);
+}
+
 static void cpsw_adjust_link(struct eth_device *edev)
 {
 	struct cpsw_slave *slave = edev->priv;
@@ -657,6 +674,8 @@ static void cpsw_slave_init(struct cpsw_slave *slave, struct cpsw_priv *priv)
 
 	/* add broadcast address */
 	cpsw_ale_add_mcast(priv, ethbdaddr, 1 << slave_port);
+
+	priv->phy_mask |= 1 << slave->phy_id;
 }
 
 static struct cpdma_desc *cpdma_desc_alloc(struct cpsw_priv *priv)
@@ -874,6 +893,9 @@ static int cpsw_send(struct eth_device *edev, void *packet, int length)
 
 	dev_dbg(priv->dev, "* %s\n", __func__);
 
+	if (!cpsw_check_link(priv))
+		return -EIO;
+
 	/* first reap completed packets */
 	while (cpdma_process(priv, &priv->tx_chan, &buffer, &len) >= 0);
 
@@ -893,6 +915,8 @@ static int cpsw_recv(struct eth_device *edev)
 	void *buffer;
 	int len;
 
+	cpsw_check_link(priv);
+
 	while (cpdma_process(priv, &priv->rx_chan, &buffer, &len) >= 0) {
 		dma_inv_range((ulong)buffer, (ulong)buffer + len);
 		net_receive(buffer, len);
-- 
1.7.10.4