[FAQ] CPSW linux Phy回环自检实现

From 7526fa471a712cfd783edcd0bcc97a4ec62a4f41 Mon Sep 17 00:00:00 2001
From: Kamel Hacene <kamel.hacene@safrangroup.com>
Date: Mon, 7 Jun 2021 08:13:47 +0000
Subject: [PATCH] net: ethernet: ti: Add phy loopback ethtool self-test for
 cpsw driver

---
 drivers/net/ethernet/ti/cpsw.c          |   3 +-
 drivers/net/ethernet/ti/cpsw_ethtool.c  | 339 ++++++++++++++++++++++++++++++++
 drivers/net/ethernet/ti/cpsw_priv.h     |   9 +
 drivers/net/ethernet/ti/davinci_cpdma.c |  10 +
 drivers/net/ethernet/ti/davinci_cpdma.h |   2 +
 5 files changed, 362 insertions(+), 1 deletion(-)

diff --git a/drivers/net/ethernet/ti/cpsw.c b/drivers/net/ethernet/ti/cpsw.c
index 5146685eddd8..8544bb193b71 100644
--- a/drivers/net/ethernet/ti/cpsw.c
+++ b/drivers/net/ethernet/ti/cpsw.c
@@ -1058,7 +1058,7 @@ static void cpsw_fifo_shp_on(struct cpsw_priv *priv, int fifo, int on)
 	writel_relaxed(val, &cpsw->regs->ptype);
 }
 
-static void _cpsw_adjust_link(struct cpsw_slave *slave,
+void _cpsw_adjust_link(struct cpsw_slave *slave,
 			      struct cpsw_priv *priv, bool *link)
 {
 	struct phy_device	*phy = slave->phy;
@@ -2497,6 +2497,7 @@ static const struct ethtool_ops cpsw_ethtool_ops = {
 	.nway_reset	= cpsw_nway_reset,
 	.get_ringparam = cpsw_get_ringparam,
 	.set_ringparam = cpsw_set_ringparam,
+	.self_test = cpsw_self_test,
 };
 
 static int cpsw_probe_dt(struct cpsw_platform_data *data,
diff --git a/drivers/net/ethernet/ti/cpsw_ethtool.c b/drivers/net/ethernet/ti/cpsw_ethtool.c
index 31248a6cc642..843d3f1ad321 100644
--- a/drivers/net/ethernet/ti/cpsw_ethtool.c
+++ b/drivers/net/ethernet/ti/cpsw_ethtool.c
@@ -16,11 +16,23 @@
 #include <linux/skbuff.h>
 
 #include "cpsw.h"
+#include "cpsw_sl.h"
 #include "cpts.h"
 #include "cpsw_ale.h"
 #include "cpsw_priv.h"
 #include "davinci_cpdma.h"
 
+#define CPSW_XMETA_OFFSET	ALIGN(sizeof(struct xdp_frame), sizeof(long))
+struct __aligned(sizeof(long)) cpsw_meta_xdp {
+	struct net_device *ndev;
+	int ch;
+};
+#define CPSW_HEADROOM_NA (max(XDP_PACKET_HEADROOM, NET_SKB_PAD) + NET_IP_ALIGN)
+#define CPSW_HEADROOM  ALIGN(CPSW_HEADROOM_NA, sizeof(long))
+
+#define TST_FRAME_SIZE 512
+#define TST_PATTERN    0xAA
+
 struct cpsw_hw_stats {
 	u32	rxgoodframes;
 	u32	rxbroadcastframes;
@@ -217,6 +229,14 @@ int cpsw_set_coalesce(struct net_device *ndev, struct ethtool_coalesce *coal)
 	return 0;
 }
 
+enum cpsw_diagnostics_result {
+	TEST_LOOP
+};
+static const char cpsw_gstrings_test[][ETH_GSTRING_LEN] = {
+	[TEST_LOOP] = "Loopback test (offline)"
+};
+#define CPSW_TEST_LEN (sizeof(cpsw_gstrings_test) / ETH_GSTRING_LEN)
+
 int cpsw_get_sset_count(struct net_device *ndev, int sset)
 {
 	struct cpsw_common *cpsw = ndev_to_cpsw(ndev);
@@ -226,6 +246,8 @@ int cpsw_get_sset_count(struct net_device *ndev, int sset)
 		return (CPSW_STATS_COMMON_LEN +
 		       (cpsw->rx_ch_num + cpsw->tx_ch_num) *
 		       CPSW_STATS_CH_LEN);
+	case ETH_SS_TEST:
+		return CPSW_TEST_LEN;
 	default:
 		return -EOPNOTSUPP;
 	}
@@ -744,3 +766,320 @@ int cpsw_get_ts_info(struct net_device *ndev, struct ethtool_ts_info *info)
 	return 0;
 }
 #endif
+
+static void cpsw_loopback_rx_handler(void *token, int len, int status)
+{
+	struct page *page = token;
+	void *pa = page_address(page);
+	struct cpsw_meta_xdp	*xmeta = pa + CPSW_XMETA_OFFSET;
+	struct cpsw_common	*cpsw = ndev_to_cpsw(xmeta->ndev);
+	struct net_device	*ndev = xmeta->ndev;
+	int			port = 0;
+	unsigned int success = 1;
+
+	struct cpsw_priv *priv;
+	struct cpsw_test *test;
+	int i;
+	unsigned char data;
+	unsigned char* pointer = pa;
+	int length;
+
+	/* Dual emac mode ? */
+	if (cpsw->data.dual_emac && status >= 0) {
+		/* Refresh ndev if port is not 0 */
+		port = CPDMA_RX_SOURCE_PORT(status);
+		if (port)
+			ndev = cpsw->slaves[--port].ndev;
+
+		/* Ignore hw headroom and vlan fields */
+		pointer += CPSW_HEADROOM;
+		pointer += CPSW_RX_VLAN_ENCAP_HDR_SIZE;
+		length = len - (CPSW_HEADROOM + CPSW_RX_VLAN_ENCAP_HDR_SIZE);
+	}
+	else
+	{
+		/* Single emac -> Ignore hw headroom only */
+		pointer += CPSW_HEADROOM;
+		length = len - CPSW_HEADROOM;
+	}
+
+	/* Retrieve slave private structures */
+	priv = netdev_priv(ndev);
+	test = &(priv->test);
+
+	netdev_info(ndev, "Loopback test rx handler call (length:%d, pattern:%#x)\n", length, TST_PATTERN);
+
+	/* (Dual emac) Verify RX port == TX port */
+	if(success == 1)
+	{
+		if (cpsw->data.dual_emac && status >= 0) {
+			if (port != test->port)
+			{
+				netdev_err(ndev, "Loopback test: Invalid RX port (%d). Expected same as TX port (%d).\n", port, test->port);
+				success = 0;
+			}
+		}
+	}
+
+	/* Verify frame size is correct */
+	if(success == 1)
+	{
+		if(length != (TST_FRAME_SIZE - CPSW_HEADROOM))
+		{
+			netdev_err(ndev, "Loopback test: Invalid length %d (expected: %d)\n", length, TST_FRAME_SIZE - CPSW_HEADROOM);
+			success = 0;
+		}
+	}
+
+	/* Verify frame data is correct */
+	if(success == 1)
+	{
+		for(i = 0; i < length; i++)
+		{
+			data = *((unsigned char*)(pointer) + i);
+			if(data != TST_PATTERN)
+			{
+				netdev_err(ndev, "Loopback test : bad data(%d) = %#x. Expected pattern %#x\n", i, data, TST_PATTERN);
+				success = 0;
+			}
+		}
+	}
+
+	/* Set global result */
+	test->success = success;
+	netdev_info(ndev, "Loopback test rx handler call result: %d\n", success);
+
+	/* Call RX handler to drop the frame to network stack */
+	(*test->handler)((void *)token, len, status);
+}
+
+static int cpsw_loopback(struct net_device *ndev)
+{
+	struct cpsw_priv *priv = netdev_priv(ndev);
+	struct cpsw_common *cpsw = priv->cpsw;
+	struct cpsw_test *test = &(priv->test);
+	struct sk_buff *skb = NULL;
+	unsigned int frame_size;
+	struct cpdma_chan *txch;
+	struct cpdma_chan *rxch;
+	int ret;
+	int retry;
+	int success;
+
+	/* Allocate test skb */
+	skb = netdev_alloc_skb(ndev, TST_FRAME_SIZE);
+	if (!skb)
+	{
+		netdev_err(ndev, "Could not allocate socket buffer\n");
+		return -1;
+	}
+
+	/* Fill */
+	skb_put(skb, TST_FRAME_SIZE);
+	frame_size = skb->len;
+	memset(skb->data, TST_PATTERN, frame_size);
+
+	/* Use txqueue 0 */
+	txch = cpsw->txv[0].ch;
+	rxch = cpsw->rxv[0].ch;
+
+	/* Backup and et RX handler */
+	test->handler = cpdma_chan_get_handler(rxch);
+	cpdma_chan_set_handler(rxch, cpsw_loopback_rx_handler);
+
+	/* Reset TX channel */
+	ret = cpdma_chan_stop(txch);
+	if (unlikely(ret != 0)) {
+		netdev_err(ndev, "Could not stop tx channel 0\n");
+	}
+	ret = cpdma_chan_start(txch);
+	if (unlikely(ret != 0)) {
+		netdev_err(ndev, "Could not start tx channel 0\n");
+	}
+
+	skb_tx_timestamp(skb);
+
+	/* Set TX port so we can test it in RX handler */
+	test->port = priv->emac_port;
+
+	/* Default succes to false */
+	success = 0;
+
+	ret = cpdma_chan_submit(txch, skb, skb->data, skb->len,
+				priv->emac_port + cpsw->data.dual_emac);
+	if (unlikely(ret != 0)) {
+		netdev_err(ndev, "Could not send packet. Err: %x\n", ret);
+	}
+	else
+	{
+		for(retry = 0; retry < 5; retry++)
+		{
+			/* Wait for packet to go from Tx to Rx */
+			msleep(200);
+
+			/* Try to read a frame, ret return the number of rode frames. In case
+			 * the value is not 0, it means our RX handler has been called and we
+			 * can retrieve the test result.
+			 */
+			ret = cpdma_chan_process(rxch, cpsw->rxv[0].budget);
+			if(ret != 0)
+			{
+				/* Retrieve result */
+				success = test->success;
+				break;
+			}
+
+			/* Dbg */
+			netdev_info(ndev, "Retry:%d ; TXstat:%ld ; RXstat:%ld \n",
+					retry, ndev->stats.tx_bytes, ndev->stats.rx_bytes);
+		}
+	}
+
+	/* Restore RX handler */
+	cpdma_chan_set_handler(rxch, test->handler);
+
+	return success;
+}
+
+static int cpsw_loopback_test(struct cpsw_slave *slave)
+{
+	struct phy_device *phy = slave->phy;
+	struct net_device *ndev = slave->ndev;
+	struct cpsw_priv  *priv = netdev_priv(ndev);
+	struct cpsw_common *cpsw = priv->cpsw;
+	bool link;
+	int ctrl1000;
+	int bypass;
+	int slave_port;
+	int i;
+	int success = 0;
+	int bmcr;
+
+	netdev_info(ndev, "Starting loopback test\n");
+	slave_port = cpsw_get_slave_port(slave->slave_num);
+
+	/* Disable CPSW interrupts */
+	cpsw_intr_disable(cpsw);
+
+	/* Backup phy BMCR configuration */
+	bmcr = phy_read(phy, MII_BMCR);
+
+	/* Set loopback, 1Gbit, full duplex, disable autoneg */
+	phy_modify(phy, MII_BMCR,
+			BMCR_LOOPBACK | BMCR_ANENABLE | BMCR_FULLDPLX | BMCR_SPEED1000 | BMCR_SPEED100,
+			BMCR_LOOPBACK | 0             | BMCR_FULLDPLX | BMCR_SPEED1000 | 0);
+
+	/* Enable CPSW_SL 1Gbit/Full */
+	slave->mac_control = CPSW_SL_CTL_GMII_EN | CPSW_SL_CTL_GIG | CPSW_SL_CTL_FULLDUPLEX;
+	cpsw_sl_ctl_set(slave->mac_sl, slave->mac_control);
+
+	/* Enable ALE forwarding */
+	cpsw_ale_control_set(cpsw->ale, slave_port,
+				   ALE_PORT_STATE,
+				   priv->port_state[slave_port]);
+
+	/* Enable master-slave manual configuration (as slave) */
+	ctrl1000 = phy_read(phy, MII_CTRL1000);
+	phy_modify(phy, MII_CTRL1000,
+			CTL1000_ENABLE_MASTER | CTL1000_AS_MASTER,
+			CTL1000_ENABLE_MASTER | 0);
+
+	/* Wait for phy to be up */
+	for(i = 0; (i < 10) && !(phy_read(phy, MII_BMSR) & BMSR_LSTATUS); i++)
+	{
+		msleep(100);
+		netdev_info(ndev, "Waiting for phy to be up (BMSR=%#x) (check bit: %d)\n",
+				phy_read(phy, MII_BMSR), BMSR_LSTATUS);
+	}
+
+	/* Enable ALE bypass mode */
+	bypass = cpsw_ale_control_get(cpsw->ale, 0, ALE_BYPASS);
+	if(!bypass)
+	{
+		cpsw_ale_control_set(cpsw->ale, 0, ALE_BYPASS, 1);
+	}
+
+	/* Loopback test */
+	success = cpsw_loopback(ndev);
+
+	/* Restore ALE bypass */
+	if(!bypass)
+	{
+		cpsw_ale_control_set(cpsw->ale, 0, ALE_BYPASS, 0);
+	}
+
+	/* Restore phy BMCR */
+	phy_write(phy, MII_BMCR, bmcr);
+
+	/* Restore phy CTRL1000 */
+	phy_write(phy, MII_CTRL1000, ctrl1000);
+
+	/* Set back speed/duplex and ALE enable */
+	_cpsw_adjust_link(slave, priv, &link);
+
+	/* Enable interrupts */
+	cpsw_intr_enable(cpsw);
+
+	return success;
+}
+
+void cpsw_self_test(struct net_device *ndev,
+    struct ethtool_test *etest, u64 *data)
+{
+	struct cpsw_priv *priv = netdev_priv(ndev);
+	struct cpsw_common *cpsw = priv->cpsw;
+	int slave_no = cpsw_slave_index(cpsw, priv);
+	bool if_running = netif_running(ndev);
+	int other_slave = slave_no == 1 ? 0 : 1;
+
+	/* Check requested tests */
+	if(etest->flags != ETH_TEST_FL_OFFLINE)
+	{
+		netdev_err(ndev, "Only offline tests are supported\n");
+		etest->flags |= ETH_TEST_FL_FAILED;
+		return;
+	}
+
+	/* Interface needs to be up */
+	if(!if_running)
+	{
+		dev_open(ndev, NULL);
+	}
+
+	/* No carrier */
+	netif_carrier_off(ndev);
+
+	/* Stop other slave */
+	if (cpsw->data.dual_emac)
+	{
+		if(netif_running(cpsw->slaves[other_slave].ndev))
+		{
+			dev_close(cpsw->slaves[other_slave].ndev);
+		}
+	}
+
+	/* Phy loopback test */
+	if (cpsw->slaves[slave_no].phy)
+	{
+		data[TEST_LOOP] = cpsw_loopback_test(&cpsw->slaves[slave_no]);
+		if(0 == data[TEST_LOOP])
+		{
+			etest->flags |= ETH_TEST_FL_FAILED;
+		}
+	}
+	else
+	{
+		/* Need a phy for loopback test */
+		netdev_err(ndev, "This test requires a phy\n");
+		etest->flags |= ETH_TEST_FL_FAILED;
+		return;
+	}
+
+	/* Restore carrier */
+	netif_carrier_on(ndev);
+	if (cpsw->data.dual_emac)
+	{
+		dev_open(cpsw->slaves[other_slave].ndev, NULL);
+	}
+}
+
diff --git a/drivers/net/ethernet/ti/cpsw_priv.h b/drivers/net/ethernet/ti/cpsw_priv.h
index 665bcaff61e2..d3e7d01134a0 100644
--- a/drivers/net/ethernet/ti/cpsw_priv.h
+++ b/drivers/net/ethernet/ti/cpsw_priv.h
@@ -350,6 +350,12 @@ struct cpsw_common {
 	struct page_pool		*page_pool[CPSW_MAX_QUEUES];
 };
 
+struct cpsw_test {
+	u32 success;
+	u32 port;
+	cpdma_handler_fn handler;
+};
+
 struct cpsw_priv {
 	struct net_device		*ndev;
 	struct device			*dev;
@@ -365,6 +371,7 @@ struct cpsw_priv {
 	struct bpf_prog			*xdp_prog;
 	struct xdp_rxq_info		xdp_rxq[CPSW_MAX_QUEUES];
 	struct xdp_attachment_info	xdpi;
+	struct cpsw_test test;
 
 	u32 emac_port;
 	struct cpsw_common *cpsw;
@@ -434,5 +441,7 @@ int cpsw_set_channels_common(struct net_device *ndev,
 			     struct ethtool_channels *chs,
 			     cpdma_handler_fn rx_handler);
 int cpsw_get_ts_info(struct net_device *ndev, struct ethtool_ts_info *info);
+void cpsw_self_test(struct net_device *ndev, struct ethtool_test *eth_test, u64 *data);
+void _cpsw_adjust_link(struct cpsw_slave *slave, struct cpsw_priv *priv, bool *link);
 
 #endif /* DRIVERS_NET_ETHERNET_TI_CPSW_PRIV_H_ */
diff --git a/drivers/net/ethernet/ti/davinci_cpdma.c b/drivers/net/ethernet/ti/davinci_cpdma.c
index 6614fa3089b2..0653dfe421c8 100644
--- a/drivers/net/ethernet/ti/davinci_cpdma.c
+++ b/drivers/net/ethernet/ti/davinci_cpdma.c
@@ -873,6 +873,16 @@ u32 cpdma_chan_get_rate(struct cpdma_chan *ch)
 	return rate;
 }
 
+cpdma_handler_fn cpdma_chan_get_handler(struct cpdma_chan *ch)
+{
+	return ch->handler;
+}
+
+void cpdma_chan_set_handler(struct cpdma_chan *ch, cpdma_handler_fn handler)
+{
+	ch->handler = handler;
+}
+
 struct cpdma_chan *cpdma_chan_create(struct cpdma_ctlr *ctlr, int chan_num,
 				     cpdma_handler_fn handler, int rx_type)
 {
diff --git a/drivers/net/ethernet/ti/davinci_cpdma.h b/drivers/net/ethernet/ti/davinci_cpdma.h
index d3cfe234d16a..b5dd2e68e024 100644
--- a/drivers/net/ethernet/ti/davinci_cpdma.h
+++ b/drivers/net/ethernet/ti/davinci_cpdma.h
@@ -97,6 +97,8 @@ int cpdma_chan_set_weight(struct cpdma_chan *ch, int weight);
 int cpdma_chan_set_rate(struct cpdma_chan *ch, u32 rate);
 u32 cpdma_chan_get_rate(struct cpdma_chan *ch);
 u32 cpdma_chan_get_min_rate(struct cpdma_ctlr *ctlr);
+cpdma_handler_fn cpdma_chan_get_handler(struct cpdma_chan *ch);
+void cpdma_chan_set_handler(struct cpdma_chan *ch, cpdma_handler_fn handler);
 
 enum cpdma_control {
 	CPDMA_TX_RLIM,			/* read-write */
-- 
2.11.0