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 };
31
32 struct net_test_priv {
33 struct net_packet_attrs *packet;
34 struct packet_type pt;
35 struct completion comp;
36 int double_vlan;
37 int vlan_id;
38 int ok;
39 };
40
41 struct netsfhdr {
42 __be32 version;
43 __be64 magic;
44 u8 id;
45 } __packed;
46
47 static u8 net_test_next_id;
48
49 #define NET_TEST_PKT_SIZE (sizeof(struct ethhdr) + sizeof(struct iphdr) + \
50 sizeof(struct netsfhdr))
51 #define NET_TEST_PKT_MAGIC 0xdeadcafecafedeadULL
52 #define NET_LB_TIMEOUT msecs_to_jiffies(200)
53
net_test_get_skb(struct net_device * ndev,struct net_packet_attrs * attr)54 static struct sk_buff *net_test_get_skb(struct net_device *ndev,
55 struct net_packet_attrs *attr)
56 {
57 struct sk_buff *skb = NULL;
58 struct udphdr *uhdr = NULL;
59 struct tcphdr *thdr = NULL;
60 struct netsfhdr *shdr;
61 struct ethhdr *ehdr;
62 struct iphdr *ihdr;
63 int iplen, size;
64
65 size = attr->size + NET_TEST_PKT_SIZE;
66
67 if (attr->tcp)
68 size += sizeof(struct tcphdr);
69 else
70 size += sizeof(struct udphdr);
71
72 if (attr->max_size && attr->max_size > size)
73 size = attr->max_size;
74
75 skb = netdev_alloc_skb(ndev, size);
76 if (!skb)
77 return NULL;
78
79 prefetchw(skb->data);
80
81 ehdr = skb_push(skb, ETH_HLEN);
82 skb_reset_mac_header(skb);
83
84 skb_set_network_header(skb, skb->len);
85 ihdr = skb_put(skb, sizeof(*ihdr));
86
87 skb_set_transport_header(skb, skb->len);
88 if (attr->tcp)
89 thdr = skb_put(skb, sizeof(*thdr));
90 else
91 uhdr = skb_put(skb, sizeof(*uhdr));
92
93 eth_zero_addr(ehdr->h_dest);
94
95 if (attr->src)
96 ether_addr_copy(ehdr->h_source, attr->src);
97 if (attr->dst)
98 ether_addr_copy(ehdr->h_dest, attr->dst);
99
100 ehdr->h_proto = htons(ETH_P_IP);
101
102 if (attr->tcp) {
103 memset(thdr, 0, sizeof(*thdr));
104 thdr->source = htons(attr->sport);
105 thdr->dest = htons(attr->dport);
106 thdr->doff = sizeof(struct tcphdr) / 4;
107 } else {
108 uhdr->source = htons(attr->sport);
109 uhdr->dest = htons(attr->dport);
110 uhdr->len = htons(sizeof(*shdr) + sizeof(*uhdr) + attr->size);
111 if (attr->max_size)
112 uhdr->len = htons(attr->max_size -
113 (sizeof(*ihdr) + sizeof(*ehdr)));
114 uhdr->check = 0;
115 }
116
117 ihdr->ihl = 5;
118 ihdr->ttl = 32;
119 ihdr->version = 4;
120 if (attr->tcp)
121 ihdr->protocol = IPPROTO_TCP;
122 else
123 ihdr->protocol = IPPROTO_UDP;
124 iplen = sizeof(*ihdr) + sizeof(*shdr) + attr->size;
125 if (attr->tcp)
126 iplen += sizeof(*thdr);
127 else
128 iplen += sizeof(*uhdr);
129
130 if (attr->max_size)
131 iplen = attr->max_size - sizeof(*ehdr);
132
133 ihdr->tot_len = htons(iplen);
134 ihdr->frag_off = 0;
135 ihdr->saddr = htonl(attr->ip_src);
136 ihdr->daddr = htonl(attr->ip_dst);
137 ihdr->tos = 0;
138 ihdr->id = 0;
139 ip_send_check(ihdr);
140
141 shdr = skb_put(skb, sizeof(*shdr));
142 shdr->version = 0;
143 shdr->magic = cpu_to_be64(NET_TEST_PKT_MAGIC);
144 attr->id = net_test_next_id;
145 shdr->id = net_test_next_id++;
146
147 if (attr->size) {
148 void *payload = skb_put(skb, attr->size);
149
150 memset(payload, 0, attr->size);
151 }
152
153 if (attr->max_size && attr->max_size > skb->len) {
154 size_t pad_len = attr->max_size - skb->len;
155 void *pad = skb_put(skb, pad_len);
156
157 memset(pad, 0, pad_len);
158 }
159
160 skb->csum = 0;
161 skb->ip_summed = CHECKSUM_PARTIAL;
162 if (attr->tcp) {
163 int l4len = skb->len - skb_transport_offset(skb);
164
165 thdr->check = ~tcp_v4_check(l4len, ihdr->saddr, ihdr->daddr, 0);
166 skb->csum_start = skb_transport_header(skb) - skb->head;
167 skb->csum_offset = offsetof(struct tcphdr, check);
168 } else {
169 udp4_hwcsum(skb, ihdr->saddr, ihdr->daddr);
170 }
171
172 skb->protocol = htons(ETH_P_IP);
173 skb->pkt_type = PACKET_HOST;
174 skb->dev = ndev;
175
176 return skb;
177 }
178
net_test_loopback_validate(struct sk_buff * skb,struct net_device * ndev,struct packet_type * pt,struct net_device * orig_ndev)179 static int net_test_loopback_validate(struct sk_buff *skb,
180 struct net_device *ndev,
181 struct packet_type *pt,
182 struct net_device *orig_ndev)
183 {
184 struct net_test_priv *tpriv = pt->af_packet_priv;
185 const unsigned char *src = tpriv->packet->src;
186 const unsigned char *dst = tpriv->packet->dst;
187 struct netsfhdr *shdr;
188 struct ethhdr *ehdr;
189 struct udphdr *uhdr;
190 struct tcphdr *thdr;
191 struct iphdr *ihdr;
192
193 skb = skb_unshare(skb, GFP_ATOMIC);
194 if (!skb)
195 goto out;
196
197 if (skb_linearize(skb))
198 goto out;
199 if (skb_headlen(skb) < (NET_TEST_PKT_SIZE - ETH_HLEN))
200 goto out;
201
202 ehdr = (struct ethhdr *)skb_mac_header(skb);
203 if (dst) {
204 if (!ether_addr_equal_unaligned(ehdr->h_dest, dst))
205 goto out;
206 }
207
208 if (src) {
209 if (!ether_addr_equal_unaligned(ehdr->h_source, src))
210 goto out;
211 }
212
213 ihdr = ip_hdr(skb);
214 if (tpriv->double_vlan)
215 ihdr = (struct iphdr *)(skb_network_header(skb) + 4);
216
217 if (tpriv->packet->tcp) {
218 if (ihdr->protocol != IPPROTO_TCP)
219 goto out;
220
221 thdr = (struct tcphdr *)((u8 *)ihdr + 4 * ihdr->ihl);
222 if (thdr->dest != htons(tpriv->packet->dport))
223 goto out;
224
225 shdr = (struct netsfhdr *)((u8 *)thdr + sizeof(*thdr));
226 } else {
227 if (ihdr->protocol != IPPROTO_UDP)
228 goto out;
229
230 uhdr = (struct udphdr *)((u8 *)ihdr + 4 * ihdr->ihl);
231 if (uhdr->dest != htons(tpriv->packet->dport))
232 goto out;
233
234 shdr = (struct netsfhdr *)((u8 *)uhdr + sizeof(*uhdr));
235 }
236
237 if (shdr->magic != cpu_to_be64(NET_TEST_PKT_MAGIC))
238 goto out;
239 if (tpriv->packet->id != shdr->id)
240 goto out;
241
242 tpriv->ok = true;
243 complete(&tpriv->comp);
244 out:
245 kfree_skb(skb);
246 return 0;
247 }
248
__net_test_loopback(struct net_device * ndev,struct net_packet_attrs * attr)249 static int __net_test_loopback(struct net_device *ndev,
250 struct net_packet_attrs *attr)
251 {
252 struct net_test_priv *tpriv;
253 struct sk_buff *skb = NULL;
254 int ret = 0;
255
256 tpriv = kzalloc(sizeof(*tpriv), GFP_KERNEL);
257 if (!tpriv)
258 return -ENOMEM;
259
260 tpriv->ok = false;
261 init_completion(&tpriv->comp);
262
263 tpriv->pt.type = htons(ETH_P_IP);
264 tpriv->pt.func = net_test_loopback_validate;
265 tpriv->pt.dev = ndev;
266 tpriv->pt.af_packet_priv = tpriv;
267 tpriv->packet = attr;
268 dev_add_pack(&tpriv->pt);
269
270 skb = net_test_get_skb(ndev, attr);
271 if (!skb) {
272 ret = -ENOMEM;
273 goto cleanup;
274 }
275
276 ret = dev_direct_xmit(skb, attr->queue_mapping);
277 if (ret < 0) {
278 goto cleanup;
279 } else if (ret > 0) {
280 ret = -ENETUNREACH;
281 goto cleanup;
282 }
283
284 if (!attr->timeout)
285 attr->timeout = NET_LB_TIMEOUT;
286
287 wait_for_completion_timeout(&tpriv->comp, attr->timeout);
288 ret = tpriv->ok ? 0 : -ETIMEDOUT;
289
290 cleanup:
291 dev_remove_pack(&tpriv->pt);
292 kfree(tpriv);
293 return ret;
294 }
295
net_test_netif_carrier(struct net_device * ndev)296 static int net_test_netif_carrier(struct net_device *ndev)
297 {
298 return netif_carrier_ok(ndev) ? 0 : -ENOLINK;
299 }
300
net_test_phy_phydev(struct net_device * ndev)301 static int net_test_phy_phydev(struct net_device *ndev)
302 {
303 return ndev->phydev ? 0 : -EOPNOTSUPP;
304 }
305
net_test_phy_loopback_enable(struct net_device * ndev)306 static int net_test_phy_loopback_enable(struct net_device *ndev)
307 {
308 if (!ndev->phydev)
309 return -EOPNOTSUPP;
310
311 return phy_loopback(ndev->phydev, true);
312 }
313
net_test_phy_loopback_disable(struct net_device * ndev)314 static int net_test_phy_loopback_disable(struct net_device *ndev)
315 {
316 if (!ndev->phydev)
317 return -EOPNOTSUPP;
318
319 return phy_loopback(ndev->phydev, false);
320 }
321
net_test_phy_loopback_udp(struct net_device * ndev)322 static int net_test_phy_loopback_udp(struct net_device *ndev)
323 {
324 struct net_packet_attrs attr = { };
325
326 attr.dst = ndev->dev_addr;
327 return __net_test_loopback(ndev, &attr);
328 }
329
net_test_phy_loopback_udp_mtu(struct net_device * ndev)330 static int net_test_phy_loopback_udp_mtu(struct net_device *ndev)
331 {
332 struct net_packet_attrs attr = { };
333
334 attr.dst = ndev->dev_addr;
335 attr.max_size = ndev->mtu;
336 return __net_test_loopback(ndev, &attr);
337 }
338
net_test_phy_loopback_tcp(struct net_device * ndev)339 static int net_test_phy_loopback_tcp(struct net_device *ndev)
340 {
341 struct net_packet_attrs attr = { };
342
343 attr.dst = ndev->dev_addr;
344 attr.tcp = true;
345 return __net_test_loopback(ndev, &attr);
346 }
347
348 static const struct net_test {
349 char name[ETH_GSTRING_LEN];
350 int (*fn)(struct net_device *ndev);
351 } net_selftests[] = {
352 {
353 .name = "Carrier ",
354 .fn = net_test_netif_carrier,
355 }, {
356 .name = "PHY dev is present ",
357 .fn = net_test_phy_phydev,
358 }, {
359 /* This test should be done before all PHY loopback test */
360 .name = "PHY internal loopback, enable ",
361 .fn = net_test_phy_loopback_enable,
362 }, {
363 .name = "PHY internal loopback, UDP ",
364 .fn = net_test_phy_loopback_udp,
365 }, {
366 .name = "PHY internal loopback, MTU ",
367 .fn = net_test_phy_loopback_udp_mtu,
368 }, {
369 .name = "PHY internal loopback, TCP ",
370 .fn = net_test_phy_loopback_tcp,
371 }, {
372 /* This test should be done after all PHY loopback test */
373 .name = "PHY internal loopback, disable",
374 .fn = net_test_phy_loopback_disable,
375 },
376 };
377
net_selftest(struct net_device * ndev,struct ethtool_test * etest,u64 * buf)378 void net_selftest(struct net_device *ndev, struct ethtool_test *etest, u64 *buf)
379 {
380 int count = net_selftest_get_count();
381 int i;
382
383 memset(buf, 0, sizeof(*buf) * count);
384 net_test_next_id = 0;
385
386 if (etest->flags != ETH_TEST_FL_OFFLINE) {
387 netdev_err(ndev, "Only offline tests are supported\n");
388 etest->flags |= ETH_TEST_FL_FAILED;
389 return;
390 }
391
392
393 for (i = 0; i < count; i++) {
394 buf[i] = net_selftests[i].fn(ndev);
395 if (buf[i] && (buf[i] != -EOPNOTSUPP))
396 etest->flags |= ETH_TEST_FL_FAILED;
397 }
398 }
399 EXPORT_SYMBOL_GPL(net_selftest);
400
net_selftest_get_count(void)401 int net_selftest_get_count(void)
402 {
403 return ARRAY_SIZE(net_selftests);
404 }
405 EXPORT_SYMBOL_GPL(net_selftest_get_count);
406
net_selftest_get_strings(u8 * data)407 void net_selftest_get_strings(u8 *data)
408 {
409 int i;
410
411 for (i = 0; i < net_selftest_get_count(); i++)
412 ethtool_sprintf(&data, "%2d. %s", i + 1,
413 net_selftests[i].name);
414 }
415 EXPORT_SYMBOL_GPL(net_selftest_get_strings);
416
417 MODULE_DESCRIPTION("Common library for generic PHY ethtool selftests");
418 MODULE_LICENSE("GPL v2");
419 MODULE_AUTHOR("Oleksij Rempel <o.rempel@pengutronix.de>");
420