// SPDX-License-Identifier: GPL-2.0/* * Copyright (c) 2019 Synopsys, Inc. and/or its affiliates. * stmmac Selftests Support * * Author: Jose Abreu <joabreu@synopsys.com> * * Ported from stmmac by: * Copyright (C) 2021 Oleksij Rempel <o.rempel@pengutronix.de> */ #include <linux/phy.h>#include <net/selftests.h>#include <net/tcp.h>#include <net/udp.h> static u8 net_test_next_id; struct sk_buff *net_test_get_skb(struct net_device *ndev, u8 id,				 struct net_packet_attrs *attr){	struct sk_buff *skb = NULL;	struct udphdr *uhdr = NULL;	struct tcphdr *thdr = NULL;	struct netsfhdr *shdr;	struct ethhdr *ehdr;	struct iphdr *ihdr;	int iplen, size; 	size = attr->size + NET_TEST_PKT_SIZE; 	if (attr->tcp)		size += sizeof(struct tcphdr);	else		size += sizeof(struct udphdr); 	if (attr->max_size && attr->max_size > size)		size = attr->max_size; 	skb = netdev_alloc_skb(ndev, size);	if (!skb)		return NULL; 	prefetchw(skb->data); 	ehdr = skb_push(skb, ETH_HLEN);	skb_reset_mac_header(skb); 	skb_set_network_header(skb, skb->len);	ihdr = skb_put(skb, sizeof(*ihdr)); 	skb_set_transport_header(skb, skb->len);	if (attr->tcp)		thdr = skb_put(skb, sizeof(*thdr));	else		uhdr = skb_put(skb, sizeof(*uhdr)); 	eth_zero_addr(ehdr->h_dest); 	if (attr->src)		ether_addr_copy(ehdr->h_source, attr->src);	if (attr->dst)		ether_addr_copy(ehdr->h_dest, attr->dst); 	ehdr->h_proto = htons(ETH_P_IP); 	if (attr->tcp) {		memset(thdr, 0, sizeof(*thdr));		thdr->source = htons(attr->sport);		thdr->dest = htons(attr->dport);		thdr->doff = sizeof(struct tcphdr) / 4;	} else {		uhdr->source = htons(attr->sport);		uhdr->dest = htons(attr->dport);		udp_set_len_short(uhdr, sizeof(*shdr) + sizeof(*uhdr) + attr->size);		if (attr->max_size)			udp_set_len_short(uhdr, attr->max_size -					  (sizeof(*ihdr) + sizeof(*ehdr)));		uhdr->check = 0;	} 	ihdr->ihl = 5;	ihdr->ttl = 32;	ihdr->version = 4;	if (attr->tcp)		ihdr->protocol = IPPROTO_TCP;	else		ihdr->protocol = IPPROTO_UDP;	iplen = sizeof(*ihdr) + sizeof(*shdr) + attr->size;	if (attr->tcp)		iplen += sizeof(*thdr);	else		iplen += sizeof(*uhdr); 	if (attr->max_size)		iplen = attr->max_size - sizeof(*ehdr); 	ihdr->tot_len = htons(iplen);	ihdr->frag_off = 0;	ihdr->saddr = htonl(attr->ip_src);	ihdr->daddr = htonl(attr->ip_dst);	ihdr->tos = 0;	ihdr->id = 0;	ip_send_check(ihdr); 	shdr = skb_put(skb, sizeof(*shdr));	shdr->version = 0;	shdr->magic = cpu_to_be64(NET_TEST_PKT_MAGIC);	attr->id = id;	shdr->id = id; 	if (attr->size) {		void *payload = skb_put(skb, attr->size); 		memset(payload, 0, attr->size);	} 	if (attr->max_size && attr->max_size > skb->len) {		size_t pad_len = attr->max_size - skb->len;		void *pad = skb_put(skb, pad_len); 		memset(pad, 0, pad_len);	} 	skb->csum = 0;	skb->ip_summed = CHECKSUM_PARTIAL;	if (attr->tcp) {		int l4len = skb->len - skb_transport_offset(skb); 		thdr->check = ~tcp_v4_check(l4len, ihdr->saddr, ihdr->daddr, 0);		skb->csum_start = skb_transport_header(skb) - skb->head;		skb->csum_offset = offsetof(struct tcphdr, check); 		if (attr->bad_csum) {			/* Force mangled checksum */			if (skb_checksum_help(skb)) {				kfree_skb(skb);				return NULL;			} 			if (thdr->check != CSUM_MANGLED_0)				thdr->check = CSUM_MANGLED_0;			else				thdr->check = csum16_sub(thdr->check,							 cpu_to_be16(1));		}	} else {		udp4_hwcsum(skb, ihdr->saddr, ihdr->daddr);	} 	skb->protocol = htons(ETH_P_IP);	skb->pkt_type = PACKET_HOST;	skb->dev = ndev; 	return skb;}EXPORT_SYMBOL_GPL(net_test_get_skb); static int net_test_loopback_validate(struct sk_buff *skb,				      struct net_device *ndev,				      struct packet_type *pt,				      struct net_device *orig_ndev){	struct net_test_priv *tpriv = pt->af_packet_priv;	const unsigned char *src = tpriv->packet->src;	const unsigned char *dst = tpriv->packet->dst;	struct netsfhdr *shdr;	struct ethhdr *ehdr;	struct udphdr *uhdr;	struct tcphdr *thdr;	struct iphdr *ihdr; 	skb = skb_unshare(skb, GFP_ATOMIC);	if (!skb)		goto out; 	if (skb_linearize(skb))		goto out;	if (skb_headlen(skb) < (NET_TEST_PKT_SIZE - ETH_HLEN))		goto out; 	ehdr = (struct ethhdr *)skb_mac_header(skb);	if (dst) {		if (!ether_addr_equal_unaligned(ehdr->h_dest, dst))			goto out;	} 	if (src) {		if (!ether_addr_equal_unaligned(ehdr->h_source, src))			goto out;	} 	ihdr = ip_hdr(skb);	if (tpriv->double_vlan)		ihdr = (struct iphdr *)(skb_network_header(skb) + 4); 	if (tpriv->packet->tcp) {		if (ihdr->protocol != IPPROTO_TCP)			goto out; 		thdr = (struct tcphdr *)((u8 *)ihdr + 4 * ihdr->ihl);		if (thdr->dest != htons(tpriv->packet->dport))			goto out; 		shdr = (struct netsfhdr *)((u8 *)thdr + sizeof(*thdr));	} else {		if (ihdr->protocol != IPPROTO_UDP)			goto out; 		uhdr = (struct udphdr *)((u8 *)ihdr + 4 * ihdr->ihl);		if (uhdr->dest != htons(tpriv->packet->dport))			goto out; 		shdr = (struct netsfhdr *)((u8 *)uhdr + sizeof(*uhdr));	} 	if (shdr->magic != cpu_to_be64(NET_TEST_PKT_MAGIC))		goto out;	if (tpriv->packet->id != shdr->id)		goto out; 	if (tpriv->packet->bad_csum && skb->ip_summed == CHECKSUM_UNNECESSARY)		tpriv->ok = -EIO;	else		tpriv->ok = true; 	complete(&tpriv->comp);out:	kfree_skb(skb);	return 0;} static int __net_test_loopback(struct net_device *ndev,			       struct net_packet_attrs *attr){	struct net_test_priv *tpriv;	struct sk_buff *skb = NULL;	int ret = 0; 	tpriv = kzalloc_obj(*tpriv);	if (!tpriv)		return -ENOMEM; 	tpriv->ok = false;	init_completion(&tpriv->comp); 	tpriv->pt.type = htons(ETH_P_IP);	tpriv->pt.func = net_test_loopback_validate;	tpriv->pt.dev = ndev;	tpriv->pt.af_packet_priv = tpriv;	tpriv->packet = attr;	dev_add_pack(&tpriv->pt); 	skb = net_test_get_skb(ndev, net_test_next_id, attr);	if (!skb) {		ret = -ENOMEM;		goto cleanup;	} 	net_test_next_id++;	ret = dev_direct_xmit(skb, attr->queue_mapping);	if (ret < 0) {		goto cleanup;	} else if (ret > 0) {		ret = -ENETUNREACH;		goto cleanup;	} 	if (!attr->timeout)		attr->timeout = NET_LB_TIMEOUT; 	wait_for_completion_timeout(&tpriv->comp, attr->timeout);	if (tpriv->ok < 0)		ret = tpriv->ok;	else if (!tpriv->ok)		ret = -ETIMEDOUT;	else		ret = 0; cleanup:	dev_remove_pack(&tpriv->pt);	kfree(tpriv);	return ret;} static int net_test_netif_carrier(struct net_device *ndev){	return netif_carrier_ok(ndev) ? 0 : -ENOLINK;} static int net_test_phy_phydev(struct net_device *ndev){	return ndev->phydev ? 0 : -EOPNOTSUPP;} static int net_test_phy_loopback_enable(struct net_device *ndev){	if (!ndev->phydev)		return -EOPNOTSUPP; 	return phy_loopback(ndev->phydev, true, 0);} static int net_test_phy_loopback_disable(struct net_device *ndev){	if (!ndev->phydev)		return -EOPNOTSUPP; 	return phy_loopback(ndev->phydev, false, 0);} static int net_test_phy_loopback_udp(struct net_device *ndev){	struct net_packet_attrs attr = { }; 	attr.dst = ndev->dev_addr;	return __net_test_loopback(ndev, &attr);} static int net_test_phy_loopback_udp_mtu(struct net_device *ndev){	struct net_packet_attrs attr = { }; 	attr.dst = ndev->dev_addr;	attr.max_size = ndev->mtu;	return __net_test_loopback(ndev, &attr);} static int net_test_phy_loopback_tcp(struct net_device *ndev){	struct net_packet_attrs attr = { }; 	attr.dst = ndev->dev_addr;	attr.tcp = true;	return __net_test_loopback(ndev, &attr);} /** * net_test_phy_loopback_tcp_bad_csum - PHY loopback test with a deliberately *					corrupted TCP checksum * @ndev: the network device to test * * Builds the same minimal Ethernet/IPv4/TCP frame as * net_test_phy_loopback_tcp(), then flips the least-significant bit of the TCP * checksum so the resulting value is provably invalid (neither 0 nor 0xFFFF). * The frame is transmitted through the device’s internal PHY loopback path: * *   test code -> MAC driver -> MAC HW -> xMII -> PHY -> *   internal PHY loopback -> xMII -> MAC HW -> MAC driver -> test code * * Result interpretation * --------------------- *  0            The frame is delivered to the stack and the driver reports *               ip_summed as CHECKSUM_NONE or CHECKSUM_COMPLETE - both are *               valid ways to indicate “bad checksum, let the stack verify.” *  -ETIMEDOUT   The MAC/PHY silently dropped the frame; hardware checksum *               verification filtered it out before the driver saw it. *  -EIO         The driver returned the frame with ip_summed == *               CHECKSUM_UNNECESSARY, falsely claiming a valid checksum and *               indicating a serious RX-path defect. * * Return: 0 on success or a negative error code on failure. */static int net_test_phy_loopback_tcp_bad_csum(struct net_device *ndev){	struct net_packet_attrs attr = { }; 	attr.dst = ndev->dev_addr;	attr.tcp = true;	attr.bad_csum = true;	return __net_test_loopback(ndev, &attr);} static const struct net_test {	char name[ETH_GSTRING_LEN];	int (*fn)(struct net_device *ndev);} net_selftests[] = {	{		.name = "Carrier                       ",		.fn = net_test_netif_carrier,	}, {		.name = "PHY dev is present            ",		.fn = net_test_phy_phydev,	}, {		/* This test should be done before all PHY loopback test */		.name = "PHY internal loopback, enable ",		.fn = net_test_phy_loopback_enable,	}, {		.name = "PHY internal loopback, UDP    ",		.fn = net_test_phy_loopback_udp,	}, {		.name = "PHY internal loopback, MTU    ",		.fn = net_test_phy_loopback_udp_mtu,	}, {		.name = "PHY internal loopback, TCP    ",		.fn = net_test_phy_loopback_tcp,	}, {		.name = "PHY loopback, bad TCP csum    ",		.fn = net_test_phy_loopback_tcp_bad_csum,	}, {		/* This test should be done after all PHY loopback test */		.name = "PHY internal loopback, disable",		.fn = net_test_phy_loopback_disable,	},}; void net_selftest(struct net_device *ndev, struct ethtool_test *etest, u64 *buf){	int count = net_selftest_get_count();	int i; 	memset(buf, 0, sizeof(*buf) * count);	net_test_next_id = 0; 	if (etest->flags != ETH_TEST_FL_OFFLINE) {		netdev_err(ndev, "Only offline tests are supported\n");		etest->flags |= ETH_TEST_FL_FAILED;		return;	}  	for (i = 0; i < count; i++) {		buf[i] = net_selftests[i].fn(ndev);		if (buf[i] && (buf[i] != -EOPNOTSUPP))			etest->flags |= ETH_TEST_FL_FAILED;	}}EXPORT_SYMBOL_GPL(net_selftest); int net_selftest_get_count(void){	return ARRAY_SIZE(net_selftests);}EXPORT_SYMBOL_GPL(net_selftest_get_count); void net_selftest_get_strings(u8 *data){	int i; 	for (i = 0; i < net_selftest_get_count(); i++)		ethtool_sprintf(&data, "%2d. %s", i + 1,				net_selftests[i].name);}EXPORT_SYMBOL_GPL(net_selftest_get_strings); MODULE_DESCRIPTION("Common library for generic PHY ethtool selftests");MODULE_LICENSE("GPL v2");MODULE_AUTHOR("Oleksij Rempel <o.rempel@pengutronix.de>");