selftests.c

  1// SPDX-License-Identifier: GPL-2.0
  2/*
  3 * Copyright (c) 2019 Synopsys, Inc. and/or its affiliates.
  4 * stmmac Selftests Support
  5 *
  6 * Author: Jose Abreu <joabreu@synopsys.com>
  7 *
  8 * Ported from stmmac by:
  9 * Copyright (C) 2021 Oleksij Rempel <o.rempel@pengutronix.de>
 10 */
 11
 12#include <linux/phy.h>
 13#include <net/selftests.h>
 14#include <net/tcp.h>
 15#include <net/udp.h>
 16
 17static u8 net_test_next_id;
 18
 19struct sk_buff *net_test_get_skb(struct net_device *ndev, u8 id,
 20				 struct net_packet_attrs *attr)
 21{
 22	struct sk_buff *skb = NULL;
 23	struct udphdr *uhdr = NULL;
 24	struct tcphdr *thdr = NULL;
 25	struct netsfhdr *shdr;
 26	struct ethhdr *ehdr;
 27	struct iphdr *ihdr;
 28	int iplen, size;
 29
 30	size = attr->size + NET_TEST_PKT_SIZE;
 31
 32	if (attr->tcp)
 33		size += sizeof(struct tcphdr);
 34	else
 35		size += sizeof(struct udphdr);
 36
 37	if (attr->max_size && attr->max_size > size)
 38		size = attr->max_size;
 39
 40	skb = netdev_alloc_skb(ndev, size);
 41	if (!skb)
 42		return NULL;
 43
 44	prefetchw(skb->data);
 45
 46	ehdr = skb_push(skb, ETH_HLEN);
 47	skb_reset_mac_header(skb);
 48
 49	skb_set_network_header(skb, skb->len);
 50	ihdr = skb_put(skb, sizeof(*ihdr));
 51
 52	skb_set_transport_header(skb, skb->len);
 53	if (attr->tcp)
 54		thdr = skb_put(skb, sizeof(*thdr));
 55	else
 56		uhdr = skb_put(skb, sizeof(*uhdr));
 57
 58	eth_zero_addr(ehdr->h_dest);
 59
 60	if (attr->src)
 61		ether_addr_copy(ehdr->h_source, attr->src);
 62	if (attr->dst)
 63		ether_addr_copy(ehdr->h_dest, attr->dst);
 64
 65	ehdr->h_proto = htons(ETH_P_IP);
 66
 67	if (attr->tcp) {
 68		memset(thdr, 0, sizeof(*thdr));
 69		thdr->source = htons(attr->sport);
 70		thdr->dest = htons(attr->dport);
 71		thdr->doff = sizeof(struct tcphdr) / 4;
 72	} else {
 73		uhdr->source = htons(attr->sport);
 74		uhdr->dest = htons(attr->dport);
 75		uhdr->len = htons(sizeof(*shdr) + sizeof(*uhdr) + attr->size);
 76		if (attr->max_size)
 77			uhdr->len = htons(attr->max_size -
 78					  (sizeof(*ihdr) + sizeof(*ehdr)));
 79		uhdr->check = 0;
 80	}
 81
 82	ihdr->ihl = 5;
 83	ihdr->ttl = 32;
 84	ihdr->version = 4;
 85	if (attr->tcp)
 86		ihdr->protocol = IPPROTO_TCP;
 87	else
 88		ihdr->protocol = IPPROTO_UDP;
 89	iplen = sizeof(*ihdr) + sizeof(*shdr) + attr->size;
 90	if (attr->tcp)
 91		iplen += sizeof(*thdr);
 92	else
 93		iplen += sizeof(*uhdr);
 94
 95	if (attr->max_size)
 96		iplen = attr->max_size - sizeof(*ehdr);
 97
 98	ihdr->tot_len = htons(iplen);
 99	ihdr->frag_off = 0;
100	ihdr->saddr = htonl(attr->ip_src);
101	ihdr->daddr = htonl(attr->ip_dst);
102	ihdr->tos = 0;
103	ihdr->id = 0;
104	ip_send_check(ihdr);
105
106	shdr = skb_put(skb, sizeof(*shdr));
107	shdr->version = 0;
108	shdr->magic = cpu_to_be64(NET_TEST_PKT_MAGIC);
109	attr->id = id;
110	shdr->id = id;
111
112	if (attr->size) {
113		void *payload = skb_put(skb, attr->size);
114
115		memset(payload, 0, attr->size);
116	}
117
118	if (attr->max_size && attr->max_size > skb->len) {
119		size_t pad_len = attr->max_size - skb->len;
120		void *pad = skb_put(skb, pad_len);
121
122		memset(pad, 0, pad_len);
123	}
124
125	skb->csum = 0;
126	skb->ip_summed = CHECKSUM_PARTIAL;
127	if (attr->tcp) {
128		int l4len = skb->len - skb_transport_offset(skb);
129
130		thdr->check = ~tcp_v4_check(l4len, ihdr->saddr, ihdr->daddr, 0);
131		skb->csum_start = skb_transport_header(skb) - skb->head;
132		skb->csum_offset = offsetof(struct tcphdr, check);
133
134		if (attr->bad_csum) {
135			/* Force mangled checksum */
136			if (skb_checksum_help(skb)) {
137				kfree_skb(skb);
138				return NULL;
139			}
140
141			if (thdr->check != CSUM_MANGLED_0)
142				thdr->check = CSUM_MANGLED_0;
143			else
144				thdr->check = csum16_sub(thdr->check,
145							 cpu_to_be16(1));
146		}
147	} else {
148		udp4_hwcsum(skb, ihdr->saddr, ihdr->daddr);
149	}
150
151	skb->protocol = htons(ETH_P_IP);
152	skb->pkt_type = PACKET_HOST;
153	skb->dev = ndev;
154
155	return skb;
156}
157EXPORT_SYMBOL_GPL(net_test_get_skb);
158
159static int net_test_loopback_validate(struct sk_buff *skb,
160				      struct net_device *ndev,
161				      struct packet_type *pt,
162				      struct net_device *orig_ndev)
163{
164	struct net_test_priv *tpriv = pt->af_packet_priv;
165	const unsigned char *src = tpriv->packet->src;
166	const unsigned char *dst = tpriv->packet->dst;
167	struct netsfhdr *shdr;
168	struct ethhdr *ehdr;
169	struct udphdr *uhdr;
170	struct tcphdr *thdr;
171	struct iphdr *ihdr;
172
173	skb = skb_unshare(skb, GFP_ATOMIC);
174	if (!skb)
175		goto out;
176
177	if (skb_linearize(skb))
178		goto out;
179	if (skb_headlen(skb) < (NET_TEST_PKT_SIZE - ETH_HLEN))
180		goto out;
181
182	ehdr = (struct ethhdr *)skb_mac_header(skb);
183	if (dst) {
184		if (!ether_addr_equal_unaligned(ehdr->h_dest, dst))
185			goto out;
186	}
187
188	if (src) {
189		if (!ether_addr_equal_unaligned(ehdr->h_source, src))
190			goto out;
191	}
192
193	ihdr = ip_hdr(skb);
194	if (tpriv->double_vlan)
195		ihdr = (struct iphdr *)(skb_network_header(skb) + 4);
196
197	if (tpriv->packet->tcp) {
198		if (ihdr->protocol != IPPROTO_TCP)
199			goto out;
200
201		thdr = (struct tcphdr *)((u8 *)ihdr + 4 * ihdr->ihl);
202		if (thdr->dest != htons(tpriv->packet->dport))
203			goto out;
204
205		shdr = (struct netsfhdr *)((u8 *)thdr + sizeof(*thdr));
206	} else {
207		if (ihdr->protocol != IPPROTO_UDP)
208			goto out;
209
210		uhdr = (struct udphdr *)((u8 *)ihdr + 4 * ihdr->ihl);
211		if (uhdr->dest != htons(tpriv->packet->dport))
212			goto out;
213
214		shdr = (struct netsfhdr *)((u8 *)uhdr + sizeof(*uhdr));
215	}
216
217	if (shdr->magic != cpu_to_be64(NET_TEST_PKT_MAGIC))
218		goto out;
219	if (tpriv->packet->id != shdr->id)
220		goto out;
221
222	if (tpriv->packet->bad_csum && skb->ip_summed == CHECKSUM_UNNECESSARY)
223		tpriv->ok = -EIO;
224	else
225		tpriv->ok = true;
226
227	complete(&tpriv->comp);
228out:
229	kfree_skb(skb);
230	return 0;
231}
232
233static int __net_test_loopback(struct net_device *ndev,
234			       struct net_packet_attrs *attr)
235{
236	struct net_test_priv *tpriv;
237	struct sk_buff *skb = NULL;
238	int ret = 0;
239
240	tpriv = kzalloc(sizeof(*tpriv), GFP_KERNEL);
241	if (!tpriv)
242		return -ENOMEM;
243
244	tpriv->ok = false;
245	init_completion(&tpriv->comp);
246
247	tpriv->pt.type = htons(ETH_P_IP);
248	tpriv->pt.func = net_test_loopback_validate;
249	tpriv->pt.dev = ndev;
250	tpriv->pt.af_packet_priv = tpriv;
251	tpriv->packet = attr;
252	dev_add_pack(&tpriv->pt);
253
254	skb = net_test_get_skb(ndev, net_test_next_id, attr);
255	if (!skb) {
256		ret = -ENOMEM;
257		goto cleanup;
258	}
259
260	net_test_next_id++;
261	ret = dev_direct_xmit(skb, attr->queue_mapping);
262	if (ret < 0) {
263		goto cleanup;
264	} else if (ret > 0) {
265		ret = -ENETUNREACH;
266		goto cleanup;
267	}
268
269	if (!attr->timeout)
270		attr->timeout = NET_LB_TIMEOUT;
271
272	wait_for_completion_timeout(&tpriv->comp, attr->timeout);
273	if (tpriv->ok < 0)
274		ret = tpriv->ok;
275	else if (!tpriv->ok)
276		ret = -ETIMEDOUT;
277	else
278		ret = 0;
279
280cleanup:
281	dev_remove_pack(&tpriv->pt);
282	kfree(tpriv);
283	return ret;
284}
285
286static int net_test_netif_carrier(struct net_device *ndev)
287{
288	return netif_carrier_ok(ndev) ? 0 : -ENOLINK;
289}
290
291static int net_test_phy_phydev(struct net_device *ndev)
292{
293	return ndev->phydev ? 0 : -EOPNOTSUPP;
294}
295
296static int net_test_phy_loopback_enable(struct net_device *ndev)
297{
298	if (!ndev->phydev)
299		return -EOPNOTSUPP;
300
301	return phy_loopback(ndev->phydev, true, 0);
302}
303
304static int net_test_phy_loopback_disable(struct net_device *ndev)
305{
306	if (!ndev->phydev)
307		return -EOPNOTSUPP;
308
309	return phy_loopback(ndev->phydev, false, 0);
310}
311
312static int net_test_phy_loopback_udp(struct net_device *ndev)
313{
314	struct net_packet_attrs attr = { };
315
316	attr.dst = ndev->dev_addr;
317	return __net_test_loopback(ndev, &attr);
318}
319
320static int net_test_phy_loopback_udp_mtu(struct net_device *ndev)
321{
322	struct net_packet_attrs attr = { };
323
324	attr.dst = ndev->dev_addr;
325	attr.max_size = ndev->mtu;
326	return __net_test_loopback(ndev, &attr);
327}
328
329static int net_test_phy_loopback_tcp(struct net_device *ndev)
330{
331	struct net_packet_attrs attr = { };
332
333	attr.dst = ndev->dev_addr;
334	attr.tcp = true;
335	return __net_test_loopback(ndev, &attr);
336}
337
338/**
339 * net_test_phy_loopback_tcp_bad_csum - PHY loopback test with a deliberately
340 *					corrupted TCP checksum
341 * @ndev: the network device to test
342 *
343 * Builds the same minimal Ethernet/IPv4/TCP frame as
344 * net_test_phy_loopback_tcp(), then flips the least-significant bit of the TCP
345 * checksum so the resulting value is provably invalid (neither 0 nor 0xFFFF).
346 * The frame is transmitted through the device’s internal PHY loopback path:
347 *
348 *   test code -> MAC driver -> MAC HW -> xMII -> PHY ->
349 *   internal PHY loopback -> xMII -> MAC HW -> MAC driver -> test code
350 *
351 * Result interpretation
352 * ---------------------
353 *  0            The frame is delivered to the stack and the driver reports
354 *               ip_summed as CHECKSUM_NONE or CHECKSUM_COMPLETE - both are
355 *               valid ways to indicate “bad checksum, let the stack verify.”
356 *  -ETIMEDOUT   The MAC/PHY silently dropped the frame; hardware checksum
357 *               verification filtered it out before the driver saw it.
358 *  -EIO         The driver returned the frame with ip_summed ==
359 *               CHECKSUM_UNNECESSARY, falsely claiming a valid checksum and
360 *               indicating a serious RX-path defect.
361 *
362 * Return: 0 on success or a negative error code on failure.
363 */
364static int net_test_phy_loopback_tcp_bad_csum(struct net_device *ndev)
365{
366	struct net_packet_attrs attr = { };
367
368	attr.dst = ndev->dev_addr;
369	attr.tcp = true;
370	attr.bad_csum = true;
371	return __net_test_loopback(ndev, &attr);
372}
373
374static const struct net_test {
375	char name[ETH_GSTRING_LEN];
376	int (*fn)(struct net_device *ndev);
377} net_selftests[] = {
378	{
379		.name = "Carrier                       ",
380		.fn = net_test_netif_carrier,
381	}, {
382		.name = "PHY dev is present            ",
383		.fn = net_test_phy_phydev,
384	}, {
385		/* This test should be done before all PHY loopback test */
386		.name = "PHY internal loopback, enable ",
387		.fn = net_test_phy_loopback_enable,
388	}, {
389		.name = "PHY internal loopback, UDP    ",
390		.fn = net_test_phy_loopback_udp,
391	}, {
392		.name = "PHY internal loopback, MTU    ",
393		.fn = net_test_phy_loopback_udp_mtu,
394	}, {
395		.name = "PHY internal loopback, TCP    ",
396		.fn = net_test_phy_loopback_tcp,
397	}, {
398		.name = "PHY loopback, bad TCP csum    ",
399		.fn = net_test_phy_loopback_tcp_bad_csum,
400	}, {
401		/* This test should be done after all PHY loopback test */
402		.name = "PHY internal loopback, disable",
403		.fn = net_test_phy_loopback_disable,
404	},
405};
406
407void net_selftest(struct net_device *ndev, struct ethtool_test *etest, u64 *buf)
408{
409	int count = net_selftest_get_count();
410	int i;
411
412	memset(buf, 0, sizeof(*buf) * count);
413	net_test_next_id = 0;
414
415	if (etest->flags != ETH_TEST_FL_OFFLINE) {
416		netdev_err(ndev, "Only offline tests are supported\n");
417		etest->flags |= ETH_TEST_FL_FAILED;
418		return;
419	}
420
421
422	for (i = 0; i < count; i++) {
423		buf[i] = net_selftests[i].fn(ndev);
424		if (buf[i] && (buf[i] != -EOPNOTSUPP))
425			etest->flags |= ETH_TEST_FL_FAILED;
426	}
427}
428EXPORT_SYMBOL_GPL(net_selftest);
429
430int net_selftest_get_count(void)
431{
432	return ARRAY_SIZE(net_selftests);
433}
434EXPORT_SYMBOL_GPL(net_selftest_get_count);
435
436void net_selftest_get_strings(u8 *data)
437{
438	int i;
439
440	for (i = 0; i < net_selftest_get_count(); i++)
441		ethtool_sprintf(&data, "%2d. %s", i + 1,
442				net_selftests[i].name);
443}
444EXPORT_SYMBOL_GPL(net_selftest_get_strings);
445
446MODULE_DESCRIPTION("Common library for generic PHY ethtool selftests");
447MODULE_LICENSE("GPL v2");
448MODULE_AUTHOR("Oleksij Rempel <o.rempel@pengutronix.de>");