Commit | Line | Data |
---|---|---|
3e1e58d6 OR |
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 { | |
5fd348a0 JK |
18 | const unsigned char *src; |
19 | const unsigned char *dst; | |
3e1e58d6 OR |
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 | ||
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 | thdr->source = htons(attr->sport); | |
104 | thdr->dest = htons(attr->dport); | |
105 | thdr->doff = sizeof(struct tcphdr) / 4; | |
106 | thdr->check = 0; | |
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 | skb_put(skb, attr->size); | |
149 | if (attr->max_size && attr->max_size > skb->len) | |
150 | skb_put(skb, attr->max_size - skb->len); | |
151 | ||
152 | skb->csum = 0; | |
153 | skb->ip_summed = CHECKSUM_PARTIAL; | |
154 | if (attr->tcp) { | |
155 | thdr->check = ~tcp_v4_check(skb->len, ihdr->saddr, | |
156 | ihdr->daddr, 0); | |
157 | skb->csum_start = skb_transport_header(skb) - skb->head; | |
158 | skb->csum_offset = offsetof(struct tcphdr, check); | |
159 | } else { | |
160 | udp4_hwcsum(skb, ihdr->saddr, ihdr->daddr); | |
161 | } | |
162 | ||
163 | skb->protocol = htons(ETH_P_IP); | |
164 | skb->pkt_type = PACKET_HOST; | |
165 | skb->dev = ndev; | |
166 | ||
167 | return skb; | |
168 | } | |
169 | ||
170 | static int net_test_loopback_validate(struct sk_buff *skb, | |
171 | struct net_device *ndev, | |
172 | struct packet_type *pt, | |
173 | struct net_device *orig_ndev) | |
174 | { | |
175 | struct net_test_priv *tpriv = pt->af_packet_priv; | |
5fd348a0 JK |
176 | const unsigned char *src = tpriv->packet->src; |
177 | const unsigned char *dst = tpriv->packet->dst; | |
3e1e58d6 OR |
178 | struct netsfhdr *shdr; |
179 | struct ethhdr *ehdr; | |
180 | struct udphdr *uhdr; | |
181 | struct tcphdr *thdr; | |
182 | struct iphdr *ihdr; | |
183 | ||
184 | skb = skb_unshare(skb, GFP_ATOMIC); | |
185 | if (!skb) | |
186 | goto out; | |
187 | ||
188 | if (skb_linearize(skb)) | |
189 | goto out; | |
190 | if (skb_headlen(skb) < (NET_TEST_PKT_SIZE - ETH_HLEN)) | |
191 | goto out; | |
192 | ||
193 | ehdr = (struct ethhdr *)skb_mac_header(skb); | |
194 | if (dst) { | |
195 | if (!ether_addr_equal_unaligned(ehdr->h_dest, dst)) | |
196 | goto out; | |
197 | } | |
198 | ||
199 | if (src) { | |
200 | if (!ether_addr_equal_unaligned(ehdr->h_source, src)) | |
201 | goto out; | |
202 | } | |
203 | ||
204 | ihdr = ip_hdr(skb); | |
205 | if (tpriv->double_vlan) | |
206 | ihdr = (struct iphdr *)(skb_network_header(skb) + 4); | |
207 | ||
208 | if (tpriv->packet->tcp) { | |
209 | if (ihdr->protocol != IPPROTO_TCP) | |
210 | goto out; | |
211 | ||
212 | thdr = (struct tcphdr *)((u8 *)ihdr + 4 * ihdr->ihl); | |
213 | if (thdr->dest != htons(tpriv->packet->dport)) | |
214 | goto out; | |
215 | ||
216 | shdr = (struct netsfhdr *)((u8 *)thdr + sizeof(*thdr)); | |
217 | } else { | |
218 | if (ihdr->protocol != IPPROTO_UDP) | |
219 | goto out; | |
220 | ||
221 | uhdr = (struct udphdr *)((u8 *)ihdr + 4 * ihdr->ihl); | |
222 | if (uhdr->dest != htons(tpriv->packet->dport)) | |
223 | goto out; | |
224 | ||
225 | shdr = (struct netsfhdr *)((u8 *)uhdr + sizeof(*uhdr)); | |
226 | } | |
227 | ||
228 | if (shdr->magic != cpu_to_be64(NET_TEST_PKT_MAGIC)) | |
229 | goto out; | |
230 | if (tpriv->packet->id != shdr->id) | |
231 | goto out; | |
232 | ||
233 | tpriv->ok = true; | |
234 | complete(&tpriv->comp); | |
235 | out: | |
236 | kfree_skb(skb); | |
237 | return 0; | |
238 | } | |
239 | ||
240 | static int __net_test_loopback(struct net_device *ndev, | |
241 | struct net_packet_attrs *attr) | |
242 | { | |
243 | struct net_test_priv *tpriv; | |
244 | struct sk_buff *skb = NULL; | |
245 | int ret = 0; | |
246 | ||
247 | tpriv = kzalloc(sizeof(*tpriv), GFP_KERNEL); | |
248 | if (!tpriv) | |
249 | return -ENOMEM; | |
250 | ||
251 | tpriv->ok = false; | |
252 | init_completion(&tpriv->comp); | |
253 | ||
254 | tpriv->pt.type = htons(ETH_P_IP); | |
255 | tpriv->pt.func = net_test_loopback_validate; | |
256 | tpriv->pt.dev = ndev; | |
257 | tpriv->pt.af_packet_priv = tpriv; | |
258 | tpriv->packet = attr; | |
259 | dev_add_pack(&tpriv->pt); | |
260 | ||
261 | skb = net_test_get_skb(ndev, attr); | |
262 | if (!skb) { | |
263 | ret = -ENOMEM; | |
264 | goto cleanup; | |
265 | } | |
266 | ||
267 | ret = dev_direct_xmit(skb, attr->queue_mapping); | |
268 | if (ret < 0) { | |
269 | goto cleanup; | |
270 | } else if (ret > 0) { | |
271 | ret = -ENETUNREACH; | |
272 | goto cleanup; | |
273 | } | |
274 | ||
275 | if (!attr->timeout) | |
276 | attr->timeout = NET_LB_TIMEOUT; | |
277 | ||
278 | wait_for_completion_timeout(&tpriv->comp, attr->timeout); | |
279 | ret = tpriv->ok ? 0 : -ETIMEDOUT; | |
280 | ||
281 | cleanup: | |
282 | dev_remove_pack(&tpriv->pt); | |
283 | kfree(tpriv); | |
284 | return ret; | |
285 | } | |
286 | ||
287 | static int net_test_netif_carrier(struct net_device *ndev) | |
288 | { | |
289 | return netif_carrier_ok(ndev) ? 0 : -ENOLINK; | |
290 | } | |
291 | ||
292 | static int net_test_phy_phydev(struct net_device *ndev) | |
293 | { | |
294 | return ndev->phydev ? 0 : -EOPNOTSUPP; | |
295 | } | |
296 | ||
297 | static int net_test_phy_loopback_enable(struct net_device *ndev) | |
298 | { | |
299 | if (!ndev->phydev) | |
300 | return -EOPNOTSUPP; | |
301 | ||
302 | return phy_loopback(ndev->phydev, true); | |
303 | } | |
304 | ||
305 | static int net_test_phy_loopback_disable(struct net_device *ndev) | |
306 | { | |
307 | if (!ndev->phydev) | |
308 | return -EOPNOTSUPP; | |
309 | ||
310 | return phy_loopback(ndev->phydev, false); | |
311 | } | |
312 | ||
313 | static int net_test_phy_loopback_udp(struct net_device *ndev) | |
314 | { | |
315 | struct net_packet_attrs attr = { }; | |
316 | ||
317 | attr.dst = ndev->dev_addr; | |
318 | return __net_test_loopback(ndev, &attr); | |
319 | } | |
320 | ||
802a76af OR |
321 | static int net_test_phy_loopback_udp_mtu(struct net_device *ndev) |
322 | { | |
323 | struct net_packet_attrs attr = { }; | |
324 | ||
325 | attr.dst = ndev->dev_addr; | |
326 | attr.max_size = ndev->mtu; | |
327 | return __net_test_loopback(ndev, &attr); | |
328 | } | |
329 | ||
3e1e58d6 OR |
330 | static int net_test_phy_loopback_tcp(struct net_device *ndev) |
331 | { | |
332 | struct net_packet_attrs attr = { }; | |
333 | ||
334 | attr.dst = ndev->dev_addr; | |
335 | attr.tcp = true; | |
336 | return __net_test_loopback(ndev, &attr); | |
337 | } | |
338 | ||
339 | static const struct net_test { | |
340 | char name[ETH_GSTRING_LEN]; | |
341 | int (*fn)(struct net_device *ndev); | |
342 | } net_selftests[] = { | |
343 | { | |
344 | .name = "Carrier ", | |
345 | .fn = net_test_netif_carrier, | |
346 | }, { | |
347 | .name = "PHY dev is present ", | |
348 | .fn = net_test_phy_phydev, | |
349 | }, { | |
350 | /* This test should be done before all PHY loopback test */ | |
351 | .name = "PHY internal loopback, enable ", | |
352 | .fn = net_test_phy_loopback_enable, | |
353 | }, { | |
354 | .name = "PHY internal loopback, UDP ", | |
355 | .fn = net_test_phy_loopback_udp, | |
802a76af OR |
356 | }, { |
357 | .name = "PHY internal loopback, MTU ", | |
358 | .fn = net_test_phy_loopback_udp_mtu, | |
3e1e58d6 OR |
359 | }, { |
360 | .name = "PHY internal loopback, TCP ", | |
361 | .fn = net_test_phy_loopback_tcp, | |
362 | }, { | |
363 | /* This test should be done after all PHY loopback test */ | |
364 | .name = "PHY internal loopback, disable", | |
365 | .fn = net_test_phy_loopback_disable, | |
366 | }, | |
367 | }; | |
368 | ||
369 | void net_selftest(struct net_device *ndev, struct ethtool_test *etest, u64 *buf) | |
370 | { | |
371 | int count = net_selftest_get_count(); | |
372 | int i; | |
373 | ||
374 | memset(buf, 0, sizeof(*buf) * count); | |
375 | net_test_next_id = 0; | |
376 | ||
377 | if (etest->flags != ETH_TEST_FL_OFFLINE) { | |
378 | netdev_err(ndev, "Only offline tests are supported\n"); | |
379 | etest->flags |= ETH_TEST_FL_FAILED; | |
380 | return; | |
381 | } | |
382 | ||
383 | ||
384 | for (i = 0; i < count; i++) { | |
385 | buf[i] = net_selftests[i].fn(ndev); | |
386 | if (buf[i] && (buf[i] != -EOPNOTSUPP)) | |
387 | etest->flags |= ETH_TEST_FL_FAILED; | |
388 | } | |
389 | } | |
390 | EXPORT_SYMBOL_GPL(net_selftest); | |
391 | ||
392 | int net_selftest_get_count(void) | |
393 | { | |
394 | return ARRAY_SIZE(net_selftests); | |
395 | } | |
396 | EXPORT_SYMBOL_GPL(net_selftest_get_count); | |
397 | ||
398 | void net_selftest_get_strings(u8 *data) | |
399 | { | |
400 | u8 *p = data; | |
401 | int i; | |
402 | ||
403 | for (i = 0; i < net_selftest_get_count(); i++) { | |
404 | snprintf(p, ETH_GSTRING_LEN, "%2d. %s", i + 1, | |
405 | net_selftests[i].name); | |
406 | p += ETH_GSTRING_LEN; | |
407 | } | |
408 | } | |
409 | EXPORT_SYMBOL_GPL(net_selftest_get_strings); | |
410 | ||
411 | MODULE_LICENSE("GPL v2"); | |
412 | MODULE_AUTHOR("Oleksij Rempel <o.rempel@pengutronix.de>"); |