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 | |
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(dev: ndev, length: size); |
76 | if (!skb) |
77 | return NULL; |
78 | |
79 | prefetchw(x: skb->data); |
80 | |
81 | ehdr = skb_push(skb, ETH_HLEN); |
82 | skb_reset_mac_header(skb); |
83 | |
84 | skb_set_network_header(skb, offset: skb->len); |
85 | ihdr = skb_put(skb, len: sizeof(*ihdr)); |
86 | |
87 | skb_set_transport_header(skb, offset: skb->len); |
88 | if (attr->tcp) |
89 | thdr = skb_put(skb, len: sizeof(*thdr)); |
90 | else |
91 | uhdr = skb_put(skb, len: sizeof(*uhdr)); |
92 | |
93 | eth_zero_addr(addr: ehdr->h_dest); |
94 | |
95 | if (attr->src) |
96 | ether_addr_copy(dst: ehdr->h_source, src: attr->src); |
97 | if (attr->dst) |
98 | ether_addr_copy(dst: ehdr->h_dest, src: 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(ip: ihdr); |
140 | |
141 | shdr = skb_put(skb, len: 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, len: attr->size); |
149 | if (attr->max_size && attr->max_size > skb->len) |
150 | skb_put(skb, len: 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(len: skb->len, saddr: ihdr->saddr, |
156 | daddr: ihdr->daddr, base: 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, src: ihdr->saddr, dst: 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; |
176 | const unsigned char *src = tpriv->packet->src; |
177 | const unsigned char *dst = tpriv->packet->dst; |
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(addr1: ehdr->h_dest, addr2: dst)) |
196 | goto out; |
197 | } |
198 | |
199 | if (src) { |
200 | if (!ether_addr_equal_unaligned(addr1: ehdr->h_source, addr2: 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(size: sizeof(*tpriv), GFP_KERNEL); |
248 | if (!tpriv) |
249 | return -ENOMEM; |
250 | |
251 | tpriv->ok = false; |
252 | init_completion(x: &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(pt: &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, queue_id: 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(x: &tpriv->comp, timeout: attr->timeout); |
279 | ret = tpriv->ok ? 0 : -ETIMEDOUT; |
280 | |
281 | cleanup: |
282 | dev_remove_pack(pt: &tpriv->pt); |
283 | kfree(objp: tpriv); |
284 | return ret; |
285 | } |
286 | |
287 | static int net_test_netif_carrier(struct net_device *ndev) |
288 | { |
289 | return netif_carrier_ok(dev: 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(phydev: ndev->phydev, enable: 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(phydev: ndev->phydev, enable: 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: &attr); |
319 | } |
320 | |
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: &attr); |
328 | } |
329 | |
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: &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, |
356 | }, { |
357 | .name = "PHY internal loopback, MTU " , |
358 | .fn = net_test_phy_loopback_udp_mtu, |
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(dev: ndev, format: "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 | int i; |
401 | |
402 | for (i = 0; i < net_selftest_get_count(); i++) |
403 | ethtool_sprintf(data: &data, fmt: "%2d. %s" , i + 1, |
404 | net_selftests[i].name); |
405 | } |
406 | EXPORT_SYMBOL_GPL(net_selftest_get_strings); |
407 | |
408 | MODULE_DESCRIPTION("Common library for generic PHY ethtool selftests" ); |
409 | MODULE_LICENSE("GPL v2" ); |
410 | MODULE_AUTHOR("Oleksij Rempel <o.rempel@pengutronix.de>" ); |
411 | |