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