1
2
3
4
5
6
7
8
9
10
11
12#include <linux/phy.h>
13#include <net/selftests.h>
14#include <net/tcp.h>
15#include <net/udp.h>
16
17struct net_packet_attrs {
18 unsigned char *src;
19 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
32struct 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
41struct netsfhdr {
42 __be32 version;
43 __be64 magic;
44 u8 id;
45} __packed;
46
47static 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
54static 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
170static 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 unsigned char *src = tpriv->packet->src;
177 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(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);
235out:
236 kfree_skb(skb);
237 return 0;
238}
239
240static 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
281cleanup:
282 dev_remove_pack(&tpriv->pt);
283 kfree(tpriv);
284 return ret;
285}
286
287static int net_test_netif_carrier(struct net_device *ndev)
288{
289 return netif_carrier_ok(ndev) ? 0 : -ENOLINK;
290}
291
292static int net_test_phy_phydev(struct net_device *ndev)
293{
294 return ndev->phydev ? 0 : -EOPNOTSUPP;
295}
296
297static 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
305static 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
313static 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
321static 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
330static 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
339static 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
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
364 .name = "PHY internal loopback, disable",
365 .fn = net_test_phy_loopback_disable,
366 },
367};
368
369void 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}
390EXPORT_SYMBOL_GPL(net_selftest);
391
392int net_selftest_get_count(void)
393{
394 return ARRAY_SIZE(net_selftests);
395}
396EXPORT_SYMBOL_GPL(net_selftest_get_count);
397
398void 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}
409EXPORT_SYMBOL_GPL(net_selftest_get_strings);
410
411MODULE_LICENSE("GPL v2");
412MODULE_AUTHOR("Oleksij Rempel <o.rempel@pengutronix.de>");
413
