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
9 #include <linux/bitrev.h>
10 #include <linux/completion.h>
11 #include <linux/crc32.h>
12 #include <linux/ethtool.h>
13 #include <linux/ip.h>
14 #include <linux/phy.h>
15 #include <linux/udp.h>
16 #include <net/pkt_cls.h>
17 #include <net/tcp.h>
18 #include <net/udp.h>
19 #include <net/tc_act/tc_gact.h>
20 #include "stmmac.h"
21
22 struct stmmachdr {
23 __be32 version;
24 __be64 magic;
25 u8 id;
26 } __packed;
27
28 #define STMMAC_TEST_PKT_SIZE (sizeof(struct ethhdr) + sizeof(struct iphdr) + \
29 sizeof(struct stmmachdr))
30 #define STMMAC_TEST_PKT_MAGIC 0xdeadcafecafedeadULL
31 #define STMMAC_LB_TIMEOUT msecs_to_jiffies(200)
32
33 struct stmmac_packet_attrs {
34 int vlan;
35 int vlan_id_in;
36 int vlan_id_out;
37 unsigned char *src;
38 unsigned char *dst;
39 u32 ip_src;
40 u32 ip_dst;
41 int tcp;
42 int sport;
43 int dport;
44 u32 exp_hash;
45 int dont_wait;
46 int timeout;
47 int size;
48 int max_size;
49 int remove_sa;
50 u8 id;
51 int sarc;
52 u16 queue_mapping;
53 };
54
55 static u8 stmmac_test_next_id;
56
stmmac_test_get_udp_skb(struct stmmac_priv * priv,struct stmmac_packet_attrs * attr)57 static struct sk_buff *stmmac_test_get_udp_skb(struct stmmac_priv *priv,
58 struct stmmac_packet_attrs *attr)
59 {
60 struct sk_buff *skb = NULL;
61 struct udphdr *uhdr = NULL;
62 struct tcphdr *thdr = NULL;
63 struct stmmachdr *shdr;
64 struct ethhdr *ehdr;
65 struct iphdr *ihdr;
66 int iplen, size;
67
68 size = attr->size + STMMAC_TEST_PKT_SIZE;
69 if (attr->vlan) {
70 size += 4;
71 if (attr->vlan > 1)
72 size += 4;
73 }
74
75 if (attr->tcp)
76 size += sizeof(struct tcphdr);
77 else
78 size += sizeof(struct udphdr);
79
80 if (attr->max_size && (attr->max_size > size))
81 size = attr->max_size;
82
83 skb = netdev_alloc_skb(priv->dev, size);
84 if (!skb)
85 return NULL;
86
87 prefetchw(skb->data);
88
89 if (attr->vlan > 1)
90 ehdr = skb_push(skb, ETH_HLEN + 8);
91 else if (attr->vlan)
92 ehdr = skb_push(skb, ETH_HLEN + 4);
93 else if (attr->remove_sa)
94 ehdr = skb_push(skb, ETH_HLEN - 6);
95 else
96 ehdr = skb_push(skb, ETH_HLEN);
97 skb_reset_mac_header(skb);
98
99 skb_set_network_header(skb, skb->len);
100 ihdr = skb_put(skb, sizeof(*ihdr));
101
102 skb_set_transport_header(skb, skb->len);
103 if (attr->tcp)
104 thdr = skb_put(skb, sizeof(*thdr));
105 else
106 uhdr = skb_put(skb, sizeof(*uhdr));
107
108 if (!attr->remove_sa)
109 eth_zero_addr(ehdr->h_source);
110 eth_zero_addr(ehdr->h_dest);
111 if (attr->src && !attr->remove_sa)
112 ether_addr_copy(ehdr->h_source, attr->src);
113 if (attr->dst)
114 ether_addr_copy(ehdr->h_dest, attr->dst);
115
116 if (!attr->remove_sa) {
117 ehdr->h_proto = htons(ETH_P_IP);
118 } else {
119 __be16 *ptr = (__be16 *)ehdr;
120
121 /* HACK */
122 ptr[3] = htons(ETH_P_IP);
123 }
124
125 if (attr->vlan) {
126 __be16 *tag, *proto;
127
128 if (!attr->remove_sa) {
129 tag = (void *)ehdr + ETH_HLEN;
130 proto = (void *)ehdr + (2 * ETH_ALEN);
131 } else {
132 tag = (void *)ehdr + ETH_HLEN - 6;
133 proto = (void *)ehdr + ETH_ALEN;
134 }
135
136 proto[0] = htons(ETH_P_8021Q);
137 tag[0] = htons(attr->vlan_id_out);
138 tag[1] = htons(ETH_P_IP);
139 if (attr->vlan > 1) {
140 proto[0] = htons(ETH_P_8021AD);
141 tag[1] = htons(ETH_P_8021Q);
142 tag[2] = htons(attr->vlan_id_in);
143 tag[3] = htons(ETH_P_IP);
144 }
145 }
146
147 if (attr->tcp) {
148 thdr->source = htons(attr->sport);
149 thdr->dest = htons(attr->dport);
150 thdr->doff = sizeof(struct tcphdr) / 4;
151 thdr->check = 0;
152 } else {
153 uhdr->source = htons(attr->sport);
154 uhdr->dest = htons(attr->dport);
155 uhdr->len = htons(sizeof(*shdr) + sizeof(*uhdr) + attr->size);
156 if (attr->max_size)
157 uhdr->len = htons(attr->max_size -
158 (sizeof(*ihdr) + sizeof(*ehdr)));
159 uhdr->check = 0;
160 }
161
162 ihdr->ihl = 5;
163 ihdr->ttl = 32;
164 ihdr->version = 4;
165 if (attr->tcp)
166 ihdr->protocol = IPPROTO_TCP;
167 else
168 ihdr->protocol = IPPROTO_UDP;
169 iplen = sizeof(*ihdr) + sizeof(*shdr) + attr->size;
170 if (attr->tcp)
171 iplen += sizeof(*thdr);
172 else
173 iplen += sizeof(*uhdr);
174
175 if (attr->max_size)
176 iplen = attr->max_size - sizeof(*ehdr);
177
178 ihdr->tot_len = htons(iplen);
179 ihdr->frag_off = 0;
180 ihdr->saddr = htonl(attr->ip_src);
181 ihdr->daddr = htonl(attr->ip_dst);
182 ihdr->tos = 0;
183 ihdr->id = 0;
184 ip_send_check(ihdr);
185
186 shdr = skb_put(skb, sizeof(*shdr));
187 shdr->version = 0;
188 shdr->magic = cpu_to_be64(STMMAC_TEST_PKT_MAGIC);
189 attr->id = stmmac_test_next_id;
190 shdr->id = stmmac_test_next_id++;
191
192 if (attr->size)
193 skb_put(skb, attr->size);
194 if (attr->max_size && (attr->max_size > skb->len))
195 skb_put(skb, attr->max_size - skb->len);
196
197 skb->csum = 0;
198 skb->ip_summed = CHECKSUM_PARTIAL;
199 if (attr->tcp) {
200 thdr->check = ~tcp_v4_check(skb->len, ihdr->saddr, ihdr->daddr, 0);
201 skb->csum_start = skb_transport_header(skb) - skb->head;
202 skb->csum_offset = offsetof(struct tcphdr, check);
203 } else {
204 udp4_hwcsum(skb, ihdr->saddr, ihdr->daddr);
205 }
206
207 skb->protocol = htons(ETH_P_IP);
208 skb->pkt_type = PACKET_HOST;
209 skb->dev = priv->dev;
210
211 return skb;
212 }
213
stmmac_test_get_arp_skb(struct stmmac_priv * priv,struct stmmac_packet_attrs * attr)214 static struct sk_buff *stmmac_test_get_arp_skb(struct stmmac_priv *priv,
215 struct stmmac_packet_attrs *attr)
216 {
217 __be32 ip_src = htonl(attr->ip_src);
218 __be32 ip_dst = htonl(attr->ip_dst);
219 struct sk_buff *skb = NULL;
220
221 skb = arp_create(ARPOP_REQUEST, ETH_P_ARP, ip_dst, priv->dev, ip_src,
222 NULL, attr->src, attr->dst);
223 if (!skb)
224 return NULL;
225
226 skb->pkt_type = PACKET_HOST;
227 skb->dev = priv->dev;
228
229 return skb;
230 }
231
232 struct stmmac_test_priv {
233 struct stmmac_packet_attrs *packet;
234 struct packet_type pt;
235 struct completion comp;
236 int double_vlan;
237 int vlan_id;
238 int ok;
239 };
240
stmmac_test_loopback_validate(struct sk_buff * skb,struct net_device * ndev,struct packet_type * pt,struct net_device * orig_ndev)241 static int stmmac_test_loopback_validate(struct sk_buff *skb,
242 struct net_device *ndev,
243 struct packet_type *pt,
244 struct net_device *orig_ndev)
245 {
246 struct stmmac_test_priv *tpriv = pt->af_packet_priv;
247 unsigned char *src = tpriv->packet->src;
248 unsigned char *dst = tpriv->packet->dst;
249 struct stmmachdr *shdr;
250 struct ethhdr *ehdr;
251 struct udphdr *uhdr;
252 struct tcphdr *thdr;
253 struct iphdr *ihdr;
254
255 skb = skb_unshare(skb, GFP_ATOMIC);
256 if (!skb)
257 goto out;
258
259 if (skb_linearize(skb))
260 goto out;
261 if (skb_headlen(skb) < (STMMAC_TEST_PKT_SIZE - ETH_HLEN))
262 goto out;
263
264 ehdr = (struct ethhdr *)skb_mac_header(skb);
265 if (dst) {
266 if (!ether_addr_equal_unaligned(ehdr->h_dest, dst))
267 goto out;
268 }
269 if (tpriv->packet->sarc) {
270 if (!ether_addr_equal_unaligned(ehdr->h_source, ehdr->h_dest))
271 goto out;
272 } else if (src) {
273 if (!ether_addr_equal_unaligned(ehdr->h_source, src))
274 goto out;
275 }
276
277 ihdr = ip_hdr(skb);
278 if (tpriv->double_vlan)
279 ihdr = (struct iphdr *)(skb_network_header(skb) + 4);
280
281 if (tpriv->packet->tcp) {
282 if (ihdr->protocol != IPPROTO_TCP)
283 goto out;
284
285 thdr = (struct tcphdr *)((u8 *)ihdr + 4 * ihdr->ihl);
286 if (thdr->dest != htons(tpriv->packet->dport))
287 goto out;
288
289 shdr = (struct stmmachdr *)((u8 *)thdr + sizeof(*thdr));
290 } else {
291 if (ihdr->protocol != IPPROTO_UDP)
292 goto out;
293
294 uhdr = (struct udphdr *)((u8 *)ihdr + 4 * ihdr->ihl);
295 if (uhdr->dest != htons(tpriv->packet->dport))
296 goto out;
297
298 shdr = (struct stmmachdr *)((u8 *)uhdr + sizeof(*uhdr));
299 }
300
301 if (shdr->magic != cpu_to_be64(STMMAC_TEST_PKT_MAGIC))
302 goto out;
303 if (tpriv->packet->exp_hash && !skb->hash)
304 goto out;
305 if (tpriv->packet->id != shdr->id)
306 goto out;
307
308 tpriv->ok = true;
309 complete(&tpriv->comp);
310 out:
311 kfree_skb(skb);
312 return 0;
313 }
314
__stmmac_test_loopback(struct stmmac_priv * priv,struct stmmac_packet_attrs * attr)315 static int __stmmac_test_loopback(struct stmmac_priv *priv,
316 struct stmmac_packet_attrs *attr)
317 {
318 struct stmmac_test_priv *tpriv;
319 struct sk_buff *skb = NULL;
320 int ret = 0;
321
322 tpriv = kzalloc(sizeof(*tpriv), GFP_KERNEL);
323 if (!tpriv)
324 return -ENOMEM;
325
326 tpriv->ok = false;
327 init_completion(&tpriv->comp);
328
329 tpriv->pt.type = htons(ETH_P_IP);
330 tpriv->pt.func = stmmac_test_loopback_validate;
331 tpriv->pt.dev = priv->dev;
332 tpriv->pt.af_packet_priv = tpriv;
333 tpriv->packet = attr;
334
335 if (!attr->dont_wait)
336 dev_add_pack(&tpriv->pt);
337
338 skb = stmmac_test_get_udp_skb(priv, attr);
339 if (!skb) {
340 ret = -ENOMEM;
341 goto cleanup;
342 }
343
344 skb_set_queue_mapping(skb, attr->queue_mapping);
345 ret = dev_queue_xmit(skb);
346 if (ret)
347 goto cleanup;
348
349 if (attr->dont_wait)
350 goto cleanup;
351
352 if (!attr->timeout)
353 attr->timeout = STMMAC_LB_TIMEOUT;
354
355 wait_for_completion_timeout(&tpriv->comp, attr->timeout);
356 ret = tpriv->ok ? 0 : -ETIMEDOUT;
357
358 cleanup:
359 if (!attr->dont_wait)
360 dev_remove_pack(&tpriv->pt);
361 kfree(tpriv);
362 return ret;
363 }
364
stmmac_test_mac_loopback(struct stmmac_priv * priv)365 static int stmmac_test_mac_loopback(struct stmmac_priv *priv)
366 {
367 struct stmmac_packet_attrs attr = { };
368
369 attr.dst = priv->dev->dev_addr;
370 return __stmmac_test_loopback(priv, &attr);
371 }
372
stmmac_test_phy_loopback(struct stmmac_priv * priv)373 static int stmmac_test_phy_loopback(struct stmmac_priv *priv)
374 {
375 struct stmmac_packet_attrs attr = { };
376 int ret;
377
378 if (!priv->dev->phydev)
379 return -EBUSY;
380
381 ret = phy_loopback(priv->dev->phydev, true);
382 if (ret)
383 return ret;
384
385 attr.dst = priv->dev->dev_addr;
386 ret = __stmmac_test_loopback(priv, &attr);
387
388 phy_loopback(priv->dev->phydev, false);
389 return ret;
390 }
391
stmmac_test_mmc(struct stmmac_priv * priv)392 static int stmmac_test_mmc(struct stmmac_priv *priv)
393 {
394 struct stmmac_counters initial, final;
395 int ret;
396
397 memset(&initial, 0, sizeof(initial));
398 memset(&final, 0, sizeof(final));
399
400 if (!priv->dma_cap.rmon)
401 return -EOPNOTSUPP;
402
403 /* Save previous results into internal struct */
404 stmmac_mmc_read(priv, priv->mmcaddr, &priv->mmc);
405
406 ret = stmmac_test_mac_loopback(priv);
407 if (ret)
408 return ret;
409
410 /* These will be loopback results so no need to save them */
411 stmmac_mmc_read(priv, priv->mmcaddr, &final);
412
413 /*
414 * The number of MMC counters available depends on HW configuration
415 * so we just use this one to validate the feature. I hope there is
416 * not a version without this counter.
417 */
418 if (final.mmc_tx_framecount_g <= initial.mmc_tx_framecount_g)
419 return -EINVAL;
420
421 return 0;
422 }
423
stmmac_test_eee(struct stmmac_priv * priv)424 static int stmmac_test_eee(struct stmmac_priv *priv)
425 {
426 struct stmmac_extra_stats *initial, *final;
427 int retries = 10;
428 int ret;
429
430 if (!priv->dma_cap.eee || !priv->eee_active)
431 return -EOPNOTSUPP;
432
433 initial = kzalloc(sizeof(*initial), GFP_KERNEL);
434 if (!initial)
435 return -ENOMEM;
436
437 final = kzalloc(sizeof(*final), GFP_KERNEL);
438 if (!final) {
439 ret = -ENOMEM;
440 goto out_free_initial;
441 }
442
443 memcpy(initial, &priv->xstats, sizeof(*initial));
444
445 ret = stmmac_test_mac_loopback(priv);
446 if (ret)
447 goto out_free_final;
448
449 /* We have no traffic in the line so, sooner or later it will go LPI */
450 while (--retries) {
451 memcpy(final, &priv->xstats, sizeof(*final));
452
453 if (final->irq_tx_path_in_lpi_mode_n >
454 initial->irq_tx_path_in_lpi_mode_n)
455 break;
456 msleep(100);
457 }
458
459 if (!retries) {
460 ret = -ETIMEDOUT;
461 goto out_free_final;
462 }
463
464 if (final->irq_tx_path_in_lpi_mode_n <=
465 initial->irq_tx_path_in_lpi_mode_n) {
466 ret = -EINVAL;
467 goto out_free_final;
468 }
469
470 if (final->irq_tx_path_exit_lpi_mode_n <=
471 initial->irq_tx_path_exit_lpi_mode_n) {
472 ret = -EINVAL;
473 goto out_free_final;
474 }
475
476 out_free_final:
477 kfree(final);
478 out_free_initial:
479 kfree(initial);
480 return ret;
481 }
482
stmmac_filter_check(struct stmmac_priv * priv)483 static int stmmac_filter_check(struct stmmac_priv *priv)
484 {
485 if (!(priv->dev->flags & IFF_PROMISC))
486 return 0;
487
488 netdev_warn(priv->dev, "Test can't be run in promiscuous mode!\n");
489 return -EOPNOTSUPP;
490 }
491
stmmac_hash_check(struct stmmac_priv * priv,unsigned char * addr)492 static bool stmmac_hash_check(struct stmmac_priv *priv, unsigned char *addr)
493 {
494 int mc_offset = 32 - priv->hw->mcast_bits_log2;
495 struct netdev_hw_addr *ha;
496 u32 hash, hash_nr;
497
498 /* First compute the hash for desired addr */
499 hash = bitrev32(~crc32_le(~0, addr, 6)) >> mc_offset;
500 hash_nr = hash >> 5;
501 hash = 1 << (hash & 0x1f);
502
503 /* Now, check if it collides with any existing one */
504 netdev_for_each_mc_addr(ha, priv->dev) {
505 u32 nr = bitrev32(~crc32_le(~0, ha->addr, ETH_ALEN)) >> mc_offset;
506 if (((nr >> 5) == hash_nr) && ((1 << (nr & 0x1f)) == hash))
507 return false;
508 }
509
510 /* No collisions, address is good to go */
511 return true;
512 }
513
stmmac_perfect_check(struct stmmac_priv * priv,unsigned char * addr)514 static bool stmmac_perfect_check(struct stmmac_priv *priv, unsigned char *addr)
515 {
516 struct netdev_hw_addr *ha;
517
518 /* Check if it collides with any existing one */
519 netdev_for_each_uc_addr(ha, priv->dev) {
520 if (!memcmp(ha->addr, addr, ETH_ALEN))
521 return false;
522 }
523
524 /* No collisions, address is good to go */
525 return true;
526 }
527
stmmac_test_hfilt(struct stmmac_priv * priv)528 static int stmmac_test_hfilt(struct stmmac_priv *priv)
529 {
530 unsigned char gd_addr[ETH_ALEN] = {0xf1, 0xee, 0xdd, 0xcc, 0xbb, 0xaa};
531 unsigned char bd_addr[ETH_ALEN] = {0xf1, 0xff, 0xff, 0xff, 0xff, 0xff};
532 struct stmmac_packet_attrs attr = { };
533 int ret, tries = 256;
534
535 ret = stmmac_filter_check(priv);
536 if (ret)
537 return ret;
538
539 if (netdev_mc_count(priv->dev) >= priv->hw->multicast_filter_bins)
540 return -EOPNOTSUPP;
541
542 while (--tries) {
543 /* We only need to check the bd_addr for collisions */
544 bd_addr[ETH_ALEN - 1] = tries;
545 if (stmmac_hash_check(priv, bd_addr))
546 break;
547 }
548
549 if (!tries)
550 return -EOPNOTSUPP;
551
552 ret = dev_mc_add(priv->dev, gd_addr);
553 if (ret)
554 return ret;
555
556 attr.dst = gd_addr;
557
558 /* Shall receive packet */
559 ret = __stmmac_test_loopback(priv, &attr);
560 if (ret)
561 goto cleanup;
562
563 attr.dst = bd_addr;
564
565 /* Shall NOT receive packet */
566 ret = __stmmac_test_loopback(priv, &attr);
567 ret = ret ? 0 : -EINVAL;
568
569 cleanup:
570 dev_mc_del(priv->dev, gd_addr);
571 return ret;
572 }
573
stmmac_test_pfilt(struct stmmac_priv * priv)574 static int stmmac_test_pfilt(struct stmmac_priv *priv)
575 {
576 unsigned char gd_addr[ETH_ALEN] = {0xf0, 0x01, 0x44, 0x55, 0x66, 0x77};
577 unsigned char bd_addr[ETH_ALEN] = {0xf0, 0xff, 0xff, 0xff, 0xff, 0xff};
578 struct stmmac_packet_attrs attr = { };
579 int ret, tries = 256;
580
581 if (stmmac_filter_check(priv))
582 return -EOPNOTSUPP;
583 if (netdev_uc_count(priv->dev) >= priv->hw->unicast_filter_entries)
584 return -EOPNOTSUPP;
585
586 while (--tries) {
587 /* We only need to check the bd_addr for collisions */
588 bd_addr[ETH_ALEN - 1] = tries;
589 if (stmmac_perfect_check(priv, bd_addr))
590 break;
591 }
592
593 if (!tries)
594 return -EOPNOTSUPP;
595
596 ret = dev_uc_add(priv->dev, gd_addr);
597 if (ret)
598 return ret;
599
600 attr.dst = gd_addr;
601
602 /* Shall receive packet */
603 ret = __stmmac_test_loopback(priv, &attr);
604 if (ret)
605 goto cleanup;
606
607 attr.dst = bd_addr;
608
609 /* Shall NOT receive packet */
610 ret = __stmmac_test_loopback(priv, &attr);
611 ret = ret ? 0 : -EINVAL;
612
613 cleanup:
614 dev_uc_del(priv->dev, gd_addr);
615 return ret;
616 }
617
stmmac_test_mcfilt(struct stmmac_priv * priv)618 static int stmmac_test_mcfilt(struct stmmac_priv *priv)
619 {
620 unsigned char uc_addr[ETH_ALEN] = {0xf0, 0xff, 0xff, 0xff, 0xff, 0xff};
621 unsigned char mc_addr[ETH_ALEN] = {0xf1, 0xff, 0xff, 0xff, 0xff, 0xff};
622 struct stmmac_packet_attrs attr = { };
623 int ret, tries = 256;
624
625 if (stmmac_filter_check(priv))
626 return -EOPNOTSUPP;
627 if (netdev_uc_count(priv->dev) >= priv->hw->unicast_filter_entries)
628 return -EOPNOTSUPP;
629 if (netdev_mc_count(priv->dev) >= priv->hw->multicast_filter_bins)
630 return -EOPNOTSUPP;
631
632 while (--tries) {
633 /* We only need to check the mc_addr for collisions */
634 mc_addr[ETH_ALEN - 1] = tries;
635 if (stmmac_hash_check(priv, mc_addr))
636 break;
637 }
638
639 if (!tries)
640 return -EOPNOTSUPP;
641
642 ret = dev_uc_add(priv->dev, uc_addr);
643 if (ret)
644 return ret;
645
646 attr.dst = uc_addr;
647
648 /* Shall receive packet */
649 ret = __stmmac_test_loopback(priv, &attr);
650 if (ret)
651 goto cleanup;
652
653 attr.dst = mc_addr;
654
655 /* Shall NOT receive packet */
656 ret = __stmmac_test_loopback(priv, &attr);
657 ret = ret ? 0 : -EINVAL;
658
659 cleanup:
660 dev_uc_del(priv->dev, uc_addr);
661 return ret;
662 }
663
stmmac_test_ucfilt(struct stmmac_priv * priv)664 static int stmmac_test_ucfilt(struct stmmac_priv *priv)
665 {
666 unsigned char uc_addr[ETH_ALEN] = {0xf0, 0xff, 0xff, 0xff, 0xff, 0xff};
667 unsigned char mc_addr[ETH_ALEN] = {0xf1, 0xff, 0xff, 0xff, 0xff, 0xff};
668 struct stmmac_packet_attrs attr = { };
669 int ret, tries = 256;
670
671 if (stmmac_filter_check(priv))
672 return -EOPNOTSUPP;
673 if (netdev_uc_count(priv->dev) >= priv->hw->unicast_filter_entries)
674 return -EOPNOTSUPP;
675 if (netdev_mc_count(priv->dev) >= priv->hw->multicast_filter_bins)
676 return -EOPNOTSUPP;
677
678 while (--tries) {
679 /* We only need to check the uc_addr for collisions */
680 uc_addr[ETH_ALEN - 1] = tries;
681 if (stmmac_perfect_check(priv, uc_addr))
682 break;
683 }
684
685 if (!tries)
686 return -EOPNOTSUPP;
687
688 ret = dev_mc_add(priv->dev, mc_addr);
689 if (ret)
690 return ret;
691
692 attr.dst = mc_addr;
693
694 /* Shall receive packet */
695 ret = __stmmac_test_loopback(priv, &attr);
696 if (ret)
697 goto cleanup;
698
699 attr.dst = uc_addr;
700
701 /* Shall NOT receive packet */
702 ret = __stmmac_test_loopback(priv, &attr);
703 ret = ret ? 0 : -EINVAL;
704
705 cleanup:
706 dev_mc_del(priv->dev, mc_addr);
707 return ret;
708 }
709
stmmac_test_flowctrl_validate(struct sk_buff * skb,struct net_device * ndev,struct packet_type * pt,struct net_device * orig_ndev)710 static int stmmac_test_flowctrl_validate(struct sk_buff *skb,
711 struct net_device *ndev,
712 struct packet_type *pt,
713 struct net_device *orig_ndev)
714 {
715 struct stmmac_test_priv *tpriv = pt->af_packet_priv;
716 struct ethhdr *ehdr;
717
718 ehdr = (struct ethhdr *)skb_mac_header(skb);
719 if (!ether_addr_equal_unaligned(ehdr->h_source, orig_ndev->dev_addr))
720 goto out;
721 if (ehdr->h_proto != htons(ETH_P_PAUSE))
722 goto out;
723
724 tpriv->ok = true;
725 complete(&tpriv->comp);
726 out:
727 kfree_skb(skb);
728 return 0;
729 }
730
stmmac_test_flowctrl(struct stmmac_priv * priv)731 static int stmmac_test_flowctrl(struct stmmac_priv *priv)
732 {
733 unsigned char paddr[ETH_ALEN] = {0x01, 0x80, 0xC2, 0x00, 0x00, 0x01};
734 struct phy_device *phydev = priv->dev->phydev;
735 u32 rx_cnt = priv->plat->rx_queues_to_use;
736 struct stmmac_test_priv *tpriv;
737 unsigned int pkt_count;
738 int i, ret = 0;
739
740 if (!phydev || (!phydev->pause && !phydev->asym_pause))
741 return -EOPNOTSUPP;
742
743 tpriv = kzalloc(sizeof(*tpriv), GFP_KERNEL);
744 if (!tpriv)
745 return -ENOMEM;
746
747 tpriv->ok = false;
748 init_completion(&tpriv->comp);
749 tpriv->pt.type = htons(ETH_P_PAUSE);
750 tpriv->pt.func = stmmac_test_flowctrl_validate;
751 tpriv->pt.dev = priv->dev;
752 tpriv->pt.af_packet_priv = tpriv;
753 dev_add_pack(&tpriv->pt);
754
755 /* Compute minimum number of packets to make FIFO full */
756 pkt_count = priv->plat->rx_fifo_size;
757 if (!pkt_count)
758 pkt_count = priv->dma_cap.rx_fifo_size;
759 pkt_count /= 1400;
760 pkt_count *= 2;
761
762 for (i = 0; i < rx_cnt; i++)
763 stmmac_stop_rx(priv, priv->ioaddr, i);
764
765 ret = dev_set_promiscuity(priv->dev, 1);
766 if (ret)
767 goto cleanup;
768
769 ret = dev_mc_add(priv->dev, paddr);
770 if (ret)
771 goto cleanup;
772
773 for (i = 0; i < pkt_count; i++) {
774 struct stmmac_packet_attrs attr = { };
775
776 attr.dst = priv->dev->dev_addr;
777 attr.dont_wait = true;
778 attr.size = 1400;
779
780 ret = __stmmac_test_loopback(priv, &attr);
781 if (ret)
782 goto cleanup;
783 if (tpriv->ok)
784 break;
785 }
786
787 /* Wait for some time in case RX Watchdog is enabled */
788 msleep(200);
789
790 for (i = 0; i < rx_cnt; i++) {
791 struct stmmac_channel *ch = &priv->channel[i];
792 u32 tail;
793
794 tail = priv->rx_queue[i].dma_rx_phy +
795 (DMA_RX_SIZE * sizeof(struct dma_desc));
796
797 stmmac_set_rx_tail_ptr(priv, priv->ioaddr, tail, i);
798 stmmac_start_rx(priv, priv->ioaddr, i);
799
800 local_bh_disable();
801 napi_reschedule(&ch->rx_napi);
802 local_bh_enable();
803 }
804
805 wait_for_completion_timeout(&tpriv->comp, STMMAC_LB_TIMEOUT);
806 ret = tpriv->ok ? 0 : -ETIMEDOUT;
807
808 cleanup:
809 dev_mc_del(priv->dev, paddr);
810 dev_set_promiscuity(priv->dev, -1);
811 dev_remove_pack(&tpriv->pt);
812 kfree(tpriv);
813 return ret;
814 }
815
stmmac_test_rss(struct stmmac_priv * priv)816 static int stmmac_test_rss(struct stmmac_priv *priv)
817 {
818 struct stmmac_packet_attrs attr = { };
819
820 if (!priv->dma_cap.rssen || !priv->rss.enable)
821 return -EOPNOTSUPP;
822
823 attr.dst = priv->dev->dev_addr;
824 attr.exp_hash = true;
825 attr.sport = 0x321;
826 attr.dport = 0x123;
827
828 return __stmmac_test_loopback(priv, &attr);
829 }
830
stmmac_test_vlan_validate(struct sk_buff * skb,struct net_device * ndev,struct packet_type * pt,struct net_device * orig_ndev)831 static int stmmac_test_vlan_validate(struct sk_buff *skb,
832 struct net_device *ndev,
833 struct packet_type *pt,
834 struct net_device *orig_ndev)
835 {
836 struct stmmac_test_priv *tpriv = pt->af_packet_priv;
837 struct stmmachdr *shdr;
838 struct ethhdr *ehdr;
839 struct udphdr *uhdr;
840 struct iphdr *ihdr;
841 u16 proto;
842
843 proto = tpriv->double_vlan ? ETH_P_8021AD : ETH_P_8021Q;
844
845 skb = skb_unshare(skb, GFP_ATOMIC);
846 if (!skb)
847 goto out;
848
849 if (skb_linearize(skb))
850 goto out;
851 if (skb_headlen(skb) < (STMMAC_TEST_PKT_SIZE - ETH_HLEN))
852 goto out;
853 if (tpriv->vlan_id) {
854 if (skb->vlan_proto != htons(proto))
855 goto out;
856 if (skb->vlan_tci != tpriv->vlan_id) {
857 /* Means filter did not work. */
858 tpriv->ok = false;
859 complete(&tpriv->comp);
860 goto out;
861 }
862 }
863
864 ehdr = (struct ethhdr *)skb_mac_header(skb);
865 if (!ether_addr_equal_unaligned(ehdr->h_dest, tpriv->packet->dst))
866 goto out;
867
868 ihdr = ip_hdr(skb);
869 if (tpriv->double_vlan)
870 ihdr = (struct iphdr *)(skb_network_header(skb) + 4);
871 if (ihdr->protocol != IPPROTO_UDP)
872 goto out;
873
874 uhdr = (struct udphdr *)((u8 *)ihdr + 4 * ihdr->ihl);
875 if (uhdr->dest != htons(tpriv->packet->dport))
876 goto out;
877
878 shdr = (struct stmmachdr *)((u8 *)uhdr + sizeof(*uhdr));
879 if (shdr->magic != cpu_to_be64(STMMAC_TEST_PKT_MAGIC))
880 goto out;
881
882 tpriv->ok = true;
883 complete(&tpriv->comp);
884
885 out:
886 kfree_skb(skb);
887 return 0;
888 }
889
stmmac_test_vlanfilt(struct stmmac_priv * priv)890 static int stmmac_test_vlanfilt(struct stmmac_priv *priv)
891 {
892 struct stmmac_packet_attrs attr = { };
893 struct stmmac_test_priv *tpriv;
894 struct sk_buff *skb = NULL;
895 int ret = 0, i;
896
897 if (!priv->dma_cap.vlhash)
898 return -EOPNOTSUPP;
899
900 tpriv = kzalloc(sizeof(*tpriv), GFP_KERNEL);
901 if (!tpriv)
902 return -ENOMEM;
903
904 tpriv->ok = false;
905 init_completion(&tpriv->comp);
906
907 tpriv->pt.type = htons(ETH_P_IP);
908 tpriv->pt.func = stmmac_test_vlan_validate;
909 tpriv->pt.dev = priv->dev;
910 tpriv->pt.af_packet_priv = tpriv;
911 tpriv->packet = &attr;
912
913 /*
914 * As we use HASH filtering, false positives may appear. This is a
915 * specially chosen ID so that adjacent IDs (+4) have different
916 * HASH values.
917 */
918 tpriv->vlan_id = 0x123;
919 dev_add_pack(&tpriv->pt);
920
921 ret = vlan_vid_add(priv->dev, htons(ETH_P_8021Q), tpriv->vlan_id);
922 if (ret)
923 goto cleanup;
924
925 for (i = 0; i < 4; i++) {
926 attr.vlan = 1;
927 attr.vlan_id_out = tpriv->vlan_id + i;
928 attr.dst = priv->dev->dev_addr;
929 attr.sport = 9;
930 attr.dport = 9;
931
932 skb = stmmac_test_get_udp_skb(priv, &attr);
933 if (!skb) {
934 ret = -ENOMEM;
935 goto vlan_del;
936 }
937
938 skb_set_queue_mapping(skb, 0);
939 ret = dev_queue_xmit(skb);
940 if (ret)
941 goto vlan_del;
942
943 wait_for_completion_timeout(&tpriv->comp, STMMAC_LB_TIMEOUT);
944 ret = tpriv->ok ? 0 : -ETIMEDOUT;
945 if (ret && !i) {
946 goto vlan_del;
947 } else if (!ret && i) {
948 ret = -EINVAL;
949 goto vlan_del;
950 } else {
951 ret = 0;
952 }
953
954 tpriv->ok = false;
955 }
956
957 vlan_del:
958 vlan_vid_del(priv->dev, htons(ETH_P_8021Q), tpriv->vlan_id);
959 cleanup:
960 dev_remove_pack(&tpriv->pt);
961 kfree(tpriv);
962 return ret;
963 }
964
stmmac_test_dvlanfilt(struct stmmac_priv * priv)965 static int stmmac_test_dvlanfilt(struct stmmac_priv *priv)
966 {
967 struct stmmac_packet_attrs attr = { };
968 struct stmmac_test_priv *tpriv;
969 struct sk_buff *skb = NULL;
970 int ret = 0, i;
971
972 if (!priv->dma_cap.vlhash)
973 return -EOPNOTSUPP;
974
975 tpriv = kzalloc(sizeof(*tpriv), GFP_KERNEL);
976 if (!tpriv)
977 return -ENOMEM;
978
979 tpriv->ok = false;
980 tpriv->double_vlan = true;
981 init_completion(&tpriv->comp);
982
983 tpriv->pt.type = htons(ETH_P_8021Q);
984 tpriv->pt.func = stmmac_test_vlan_validate;
985 tpriv->pt.dev = priv->dev;
986 tpriv->pt.af_packet_priv = tpriv;
987 tpriv->packet = &attr;
988
989 /*
990 * As we use HASH filtering, false positives may appear. This is a
991 * specially chosen ID so that adjacent IDs (+4) have different
992 * HASH values.
993 */
994 tpriv->vlan_id = 0x123;
995 dev_add_pack(&tpriv->pt);
996
997 ret = vlan_vid_add(priv->dev, htons(ETH_P_8021AD), tpriv->vlan_id);
998 if (ret)
999 goto cleanup;
1000
1001 for (i = 0; i < 4; i++) {
1002 attr.vlan = 2;
1003 attr.vlan_id_out = tpriv->vlan_id + i;
1004 attr.dst = priv->dev->dev_addr;
1005 attr.sport = 9;
1006 attr.dport = 9;
1007
1008 skb = stmmac_test_get_udp_skb(priv, &attr);
1009 if (!skb) {
1010 ret = -ENOMEM;
1011 goto vlan_del;
1012 }
1013
1014 skb_set_queue_mapping(skb, 0);
1015 ret = dev_queue_xmit(skb);
1016 if (ret)
1017 goto vlan_del;
1018
1019 wait_for_completion_timeout(&tpriv->comp, STMMAC_LB_TIMEOUT);
1020 ret = tpriv->ok ? 0 : -ETIMEDOUT;
1021 if (ret && !i) {
1022 goto vlan_del;
1023 } else if (!ret && i) {
1024 ret = -EINVAL;
1025 goto vlan_del;
1026 } else {
1027 ret = 0;
1028 }
1029
1030 tpriv->ok = false;
1031 }
1032
1033 vlan_del:
1034 vlan_vid_del(priv->dev, htons(ETH_P_8021AD), tpriv->vlan_id);
1035 cleanup:
1036 dev_remove_pack(&tpriv->pt);
1037 kfree(tpriv);
1038 return ret;
1039 }
1040
1041 #ifdef CONFIG_NET_CLS_ACT
stmmac_test_rxp(struct stmmac_priv * priv)1042 static int stmmac_test_rxp(struct stmmac_priv *priv)
1043 {
1044 unsigned char addr[ETH_ALEN] = {0xde, 0xad, 0xbe, 0xef, 0x00, 0x00};
1045 struct tc_cls_u32_offload cls_u32 = { };
1046 struct stmmac_packet_attrs attr = { };
1047 struct tc_action **actions, *act;
1048 struct tc_u32_sel *sel;
1049 struct tcf_exts *exts;
1050 int ret, i, nk = 1;
1051
1052 if (!tc_can_offload(priv->dev))
1053 return -EOPNOTSUPP;
1054 if (!priv->dma_cap.frpsel)
1055 return -EOPNOTSUPP;
1056
1057 sel = kzalloc(sizeof(*sel) + nk * sizeof(struct tc_u32_key), GFP_KERNEL);
1058 if (!sel)
1059 return -ENOMEM;
1060
1061 exts = kzalloc(sizeof(*exts), GFP_KERNEL);
1062 if (!exts) {
1063 ret = -ENOMEM;
1064 goto cleanup_sel;
1065 }
1066
1067 actions = kzalloc(nk * sizeof(*actions), GFP_KERNEL);
1068 if (!actions) {
1069 ret = -ENOMEM;
1070 goto cleanup_exts;
1071 }
1072
1073 act = kzalloc(nk * sizeof(*act), GFP_KERNEL);
1074 if (!act) {
1075 ret = -ENOMEM;
1076 goto cleanup_actions;
1077 }
1078
1079 cls_u32.command = TC_CLSU32_NEW_KNODE;
1080 cls_u32.common.chain_index = 0;
1081 cls_u32.common.protocol = htons(ETH_P_ALL);
1082 cls_u32.knode.exts = exts;
1083 cls_u32.knode.sel = sel;
1084 cls_u32.knode.handle = 0x123;
1085
1086 exts->nr_actions = nk;
1087 exts->actions = actions;
1088 for (i = 0; i < nk; i++) {
1089 struct tcf_gact *gact = to_gact(&act[i]);
1090
1091 actions[i] = &act[i];
1092 gact->tcf_action = TC_ACT_SHOT;
1093 }
1094
1095 sel->nkeys = nk;
1096 sel->offshift = 0;
1097 sel->keys[0].off = 6;
1098 sel->keys[0].val = htonl(0xdeadbeef);
1099 sel->keys[0].mask = ~0x0;
1100
1101 ret = stmmac_tc_setup_cls_u32(priv, priv, &cls_u32);
1102 if (ret)
1103 goto cleanup_act;
1104
1105 attr.dst = priv->dev->dev_addr;
1106 attr.src = addr;
1107
1108 ret = __stmmac_test_loopback(priv, &attr);
1109 ret = ret ? 0 : -EINVAL; /* Shall NOT receive packet */
1110
1111 cls_u32.command = TC_CLSU32_DELETE_KNODE;
1112 stmmac_tc_setup_cls_u32(priv, priv, &cls_u32);
1113
1114 cleanup_act:
1115 kfree(act);
1116 cleanup_actions:
1117 kfree(actions);
1118 cleanup_exts:
1119 kfree(exts);
1120 cleanup_sel:
1121 kfree(sel);
1122 return ret;
1123 }
1124 #else
stmmac_test_rxp(struct stmmac_priv * priv)1125 static int stmmac_test_rxp(struct stmmac_priv *priv)
1126 {
1127 return -EOPNOTSUPP;
1128 }
1129 #endif
1130
stmmac_test_desc_sai(struct stmmac_priv * priv)1131 static int stmmac_test_desc_sai(struct stmmac_priv *priv)
1132 {
1133 unsigned char src[ETH_ALEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
1134 struct stmmac_packet_attrs attr = { };
1135 int ret;
1136
1137 if (!priv->dma_cap.vlins)
1138 return -EOPNOTSUPP;
1139
1140 attr.remove_sa = true;
1141 attr.sarc = true;
1142 attr.src = src;
1143 attr.dst = priv->dev->dev_addr;
1144
1145 priv->sarc_type = 0x1;
1146
1147 ret = __stmmac_test_loopback(priv, &attr);
1148
1149 priv->sarc_type = 0x0;
1150 return ret;
1151 }
1152
stmmac_test_desc_sar(struct stmmac_priv * priv)1153 static int stmmac_test_desc_sar(struct stmmac_priv *priv)
1154 {
1155 unsigned char src[ETH_ALEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
1156 struct stmmac_packet_attrs attr = { };
1157 int ret;
1158
1159 if (!priv->dma_cap.vlins)
1160 return -EOPNOTSUPP;
1161
1162 attr.sarc = true;
1163 attr.src = src;
1164 attr.dst = priv->dev->dev_addr;
1165
1166 priv->sarc_type = 0x2;
1167
1168 ret = __stmmac_test_loopback(priv, &attr);
1169
1170 priv->sarc_type = 0x0;
1171 return ret;
1172 }
1173
stmmac_test_reg_sai(struct stmmac_priv * priv)1174 static int stmmac_test_reg_sai(struct stmmac_priv *priv)
1175 {
1176 unsigned char src[ETH_ALEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
1177 struct stmmac_packet_attrs attr = { };
1178 int ret;
1179
1180 if (!priv->dma_cap.vlins)
1181 return -EOPNOTSUPP;
1182
1183 attr.remove_sa = true;
1184 attr.sarc = true;
1185 attr.src = src;
1186 attr.dst = priv->dev->dev_addr;
1187
1188 if (stmmac_sarc_configure(priv, priv->ioaddr, 0x2))
1189 return -EOPNOTSUPP;
1190
1191 ret = __stmmac_test_loopback(priv, &attr);
1192
1193 stmmac_sarc_configure(priv, priv->ioaddr, 0x0);
1194 return ret;
1195 }
1196
stmmac_test_reg_sar(struct stmmac_priv * priv)1197 static int stmmac_test_reg_sar(struct stmmac_priv *priv)
1198 {
1199 unsigned char src[ETH_ALEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
1200 struct stmmac_packet_attrs attr = { };
1201 int ret;
1202
1203 if (!priv->dma_cap.vlins)
1204 return -EOPNOTSUPP;
1205
1206 attr.sarc = true;
1207 attr.src = src;
1208 attr.dst = priv->dev->dev_addr;
1209
1210 if (stmmac_sarc_configure(priv, priv->ioaddr, 0x3))
1211 return -EOPNOTSUPP;
1212
1213 ret = __stmmac_test_loopback(priv, &attr);
1214
1215 stmmac_sarc_configure(priv, priv->ioaddr, 0x0);
1216 return ret;
1217 }
1218
stmmac_test_vlanoff_common(struct stmmac_priv * priv,bool svlan)1219 static int stmmac_test_vlanoff_common(struct stmmac_priv *priv, bool svlan)
1220 {
1221 struct stmmac_packet_attrs attr = { };
1222 struct stmmac_test_priv *tpriv;
1223 struct sk_buff *skb = NULL;
1224 int ret = 0;
1225 u16 proto;
1226
1227 if (!priv->dma_cap.vlins)
1228 return -EOPNOTSUPP;
1229
1230 tpriv = kzalloc(sizeof(*tpriv), GFP_KERNEL);
1231 if (!tpriv)
1232 return -ENOMEM;
1233
1234 proto = svlan ? ETH_P_8021AD : ETH_P_8021Q;
1235
1236 tpriv->ok = false;
1237 tpriv->double_vlan = svlan;
1238 init_completion(&tpriv->comp);
1239
1240 tpriv->pt.type = svlan ? htons(ETH_P_8021Q) : htons(ETH_P_IP);
1241 tpriv->pt.func = stmmac_test_vlan_validate;
1242 tpriv->pt.dev = priv->dev;
1243 tpriv->pt.af_packet_priv = tpriv;
1244 tpriv->packet = &attr;
1245 tpriv->vlan_id = 0x123;
1246 dev_add_pack(&tpriv->pt);
1247
1248 ret = vlan_vid_add(priv->dev, htons(proto), tpriv->vlan_id);
1249 if (ret)
1250 goto cleanup;
1251
1252 attr.dst = priv->dev->dev_addr;
1253
1254 skb = stmmac_test_get_udp_skb(priv, &attr);
1255 if (!skb) {
1256 ret = -ENOMEM;
1257 goto vlan_del;
1258 }
1259
1260 __vlan_hwaccel_put_tag(skb, htons(proto), tpriv->vlan_id);
1261 skb->protocol = htons(proto);
1262
1263 skb_set_queue_mapping(skb, 0);
1264 ret = dev_queue_xmit(skb);
1265 if (ret)
1266 goto vlan_del;
1267
1268 wait_for_completion_timeout(&tpriv->comp, STMMAC_LB_TIMEOUT);
1269 ret = tpriv->ok ? 0 : -ETIMEDOUT;
1270
1271 vlan_del:
1272 vlan_vid_del(priv->dev, htons(proto), tpriv->vlan_id);
1273 cleanup:
1274 dev_remove_pack(&tpriv->pt);
1275 kfree(tpriv);
1276 return ret;
1277 }
1278
stmmac_test_vlanoff(struct stmmac_priv * priv)1279 static int stmmac_test_vlanoff(struct stmmac_priv *priv)
1280 {
1281 return stmmac_test_vlanoff_common(priv, false);
1282 }
1283
stmmac_test_svlanoff(struct stmmac_priv * priv)1284 static int stmmac_test_svlanoff(struct stmmac_priv *priv)
1285 {
1286 if (!priv->dma_cap.dvlan)
1287 return -EOPNOTSUPP;
1288 return stmmac_test_vlanoff_common(priv, true);
1289 }
1290
1291 #ifdef CONFIG_NET_CLS_ACT
__stmmac_test_l3filt(struct stmmac_priv * priv,u32 dst,u32 src,u32 dst_mask,u32 src_mask)1292 static int __stmmac_test_l3filt(struct stmmac_priv *priv, u32 dst, u32 src,
1293 u32 dst_mask, u32 src_mask)
1294 {
1295 struct flow_dissector_key_ipv4_addrs key, mask;
1296 unsigned long dummy_cookie = 0xdeadbeef;
1297 struct stmmac_packet_attrs attr = { };
1298 struct flow_dissector *dissector;
1299 struct flow_cls_offload *cls;
1300 int ret, old_enable = 0;
1301 struct flow_rule *rule;
1302
1303 if (!tc_can_offload(priv->dev))
1304 return -EOPNOTSUPP;
1305 if (!priv->dma_cap.l3l4fnum)
1306 return -EOPNOTSUPP;
1307 if (priv->rss.enable) {
1308 old_enable = priv->rss.enable;
1309 priv->rss.enable = false;
1310 stmmac_rss_configure(priv, priv->hw, NULL,
1311 priv->plat->rx_queues_to_use);
1312 }
1313
1314 dissector = kzalloc(sizeof(*dissector), GFP_KERNEL);
1315 if (!dissector) {
1316 ret = -ENOMEM;
1317 goto cleanup_rss;
1318 }
1319
1320 dissector->used_keys |= (1 << FLOW_DISSECTOR_KEY_IPV4_ADDRS);
1321 dissector->offset[FLOW_DISSECTOR_KEY_IPV4_ADDRS] = 0;
1322
1323 cls = kzalloc(sizeof(*cls), GFP_KERNEL);
1324 if (!cls) {
1325 ret = -ENOMEM;
1326 goto cleanup_dissector;
1327 }
1328
1329 cls->common.chain_index = 0;
1330 cls->command = FLOW_CLS_REPLACE;
1331 cls->cookie = dummy_cookie;
1332
1333 rule = kzalloc(struct_size(rule, action.entries, 1), GFP_KERNEL);
1334 if (!rule) {
1335 ret = -ENOMEM;
1336 goto cleanup_cls;
1337 }
1338
1339 rule->match.dissector = dissector;
1340 rule->match.key = (void *)&key;
1341 rule->match.mask = (void *)&mask;
1342
1343 key.src = htonl(src);
1344 key.dst = htonl(dst);
1345 mask.src = src_mask;
1346 mask.dst = dst_mask;
1347
1348 cls->rule = rule;
1349
1350 rule->action.entries[0].id = FLOW_ACTION_DROP;
1351 rule->action.num_entries = 1;
1352
1353 attr.dst = priv->dev->dev_addr;
1354 attr.ip_dst = dst;
1355 attr.ip_src = src;
1356
1357 /* Shall receive packet */
1358 ret = __stmmac_test_loopback(priv, &attr);
1359 if (ret)
1360 goto cleanup_rule;
1361
1362 ret = stmmac_tc_setup_cls(priv, priv, cls);
1363 if (ret)
1364 goto cleanup_rule;
1365
1366 /* Shall NOT receive packet */
1367 ret = __stmmac_test_loopback(priv, &attr);
1368 ret = ret ? 0 : -EINVAL;
1369
1370 cls->command = FLOW_CLS_DESTROY;
1371 stmmac_tc_setup_cls(priv, priv, cls);
1372 cleanup_rule:
1373 kfree(rule);
1374 cleanup_cls:
1375 kfree(cls);
1376 cleanup_dissector:
1377 kfree(dissector);
1378 cleanup_rss:
1379 if (old_enable) {
1380 priv->rss.enable = old_enable;
1381 stmmac_rss_configure(priv, priv->hw, &priv->rss,
1382 priv->plat->rx_queues_to_use);
1383 }
1384
1385 return ret;
1386 }
1387 #else
__stmmac_test_l3filt(struct stmmac_priv * priv,u32 dst,u32 src,u32 dst_mask,u32 src_mask)1388 static int __stmmac_test_l3filt(struct stmmac_priv *priv, u32 dst, u32 src,
1389 u32 dst_mask, u32 src_mask)
1390 {
1391 return -EOPNOTSUPP;
1392 }
1393 #endif
1394
stmmac_test_l3filt_da(struct stmmac_priv * priv)1395 static int stmmac_test_l3filt_da(struct stmmac_priv *priv)
1396 {
1397 u32 addr = 0x10203040;
1398
1399 return __stmmac_test_l3filt(priv, addr, 0, ~0, 0);
1400 }
1401
stmmac_test_l3filt_sa(struct stmmac_priv * priv)1402 static int stmmac_test_l3filt_sa(struct stmmac_priv *priv)
1403 {
1404 u32 addr = 0x10203040;
1405
1406 return __stmmac_test_l3filt(priv, 0, addr, 0, ~0);
1407 }
1408
1409 #ifdef CONFIG_NET_CLS_ACT
__stmmac_test_l4filt(struct stmmac_priv * priv,u32 dst,u32 src,u32 dst_mask,u32 src_mask,bool udp)1410 static int __stmmac_test_l4filt(struct stmmac_priv *priv, u32 dst, u32 src,
1411 u32 dst_mask, u32 src_mask, bool udp)
1412 {
1413 struct {
1414 struct flow_dissector_key_basic bkey;
1415 struct flow_dissector_key_ports key;
1416 } __aligned(BITS_PER_LONG / 8) keys;
1417 struct {
1418 struct flow_dissector_key_basic bmask;
1419 struct flow_dissector_key_ports mask;
1420 } __aligned(BITS_PER_LONG / 8) masks;
1421 unsigned long dummy_cookie = 0xdeadbeef;
1422 struct stmmac_packet_attrs attr = { };
1423 struct flow_dissector *dissector;
1424 struct flow_cls_offload *cls;
1425 int ret, old_enable = 0;
1426 struct flow_rule *rule;
1427
1428 if (!tc_can_offload(priv->dev))
1429 return -EOPNOTSUPP;
1430 if (!priv->dma_cap.l3l4fnum)
1431 return -EOPNOTSUPP;
1432 if (priv->rss.enable) {
1433 old_enable = priv->rss.enable;
1434 priv->rss.enable = false;
1435 stmmac_rss_configure(priv, priv->hw, NULL,
1436 priv->plat->rx_queues_to_use);
1437 }
1438
1439 dissector = kzalloc(sizeof(*dissector), GFP_KERNEL);
1440 if (!dissector) {
1441 ret = -ENOMEM;
1442 goto cleanup_rss;
1443 }
1444
1445 dissector->used_keys |= (1 << FLOW_DISSECTOR_KEY_BASIC);
1446 dissector->used_keys |= (1 << FLOW_DISSECTOR_KEY_PORTS);
1447 dissector->offset[FLOW_DISSECTOR_KEY_BASIC] = 0;
1448 dissector->offset[FLOW_DISSECTOR_KEY_PORTS] = offsetof(typeof(keys), key);
1449
1450 cls = kzalloc(sizeof(*cls), GFP_KERNEL);
1451 if (!cls) {
1452 ret = -ENOMEM;
1453 goto cleanup_dissector;
1454 }
1455
1456 cls->common.chain_index = 0;
1457 cls->command = FLOW_CLS_REPLACE;
1458 cls->cookie = dummy_cookie;
1459
1460 rule = kzalloc(struct_size(rule, action.entries, 1), GFP_KERNEL);
1461 if (!rule) {
1462 ret = -ENOMEM;
1463 goto cleanup_cls;
1464 }
1465
1466 rule->match.dissector = dissector;
1467 rule->match.key = (void *)&keys;
1468 rule->match.mask = (void *)&masks;
1469
1470 keys.bkey.ip_proto = udp ? IPPROTO_UDP : IPPROTO_TCP;
1471 keys.key.src = htons(src);
1472 keys.key.dst = htons(dst);
1473 masks.mask.src = src_mask;
1474 masks.mask.dst = dst_mask;
1475
1476 cls->rule = rule;
1477
1478 rule->action.entries[0].id = FLOW_ACTION_DROP;
1479 rule->action.num_entries = 1;
1480
1481 attr.dst = priv->dev->dev_addr;
1482 attr.tcp = !udp;
1483 attr.sport = src;
1484 attr.dport = dst;
1485 attr.ip_dst = 0;
1486
1487 /* Shall receive packet */
1488 ret = __stmmac_test_loopback(priv, &attr);
1489 if (ret)
1490 goto cleanup_rule;
1491
1492 ret = stmmac_tc_setup_cls(priv, priv, cls);
1493 if (ret)
1494 goto cleanup_rule;
1495
1496 /* Shall NOT receive packet */
1497 ret = __stmmac_test_loopback(priv, &attr);
1498 ret = ret ? 0 : -EINVAL;
1499
1500 cls->command = FLOW_CLS_DESTROY;
1501 stmmac_tc_setup_cls(priv, priv, cls);
1502 cleanup_rule:
1503 kfree(rule);
1504 cleanup_cls:
1505 kfree(cls);
1506 cleanup_dissector:
1507 kfree(dissector);
1508 cleanup_rss:
1509 if (old_enable) {
1510 priv->rss.enable = old_enable;
1511 stmmac_rss_configure(priv, priv->hw, &priv->rss,
1512 priv->plat->rx_queues_to_use);
1513 }
1514
1515 return ret;
1516 }
1517 #else
__stmmac_test_l4filt(struct stmmac_priv * priv,u32 dst,u32 src,u32 dst_mask,u32 src_mask,bool udp)1518 static int __stmmac_test_l4filt(struct stmmac_priv *priv, u32 dst, u32 src,
1519 u32 dst_mask, u32 src_mask, bool udp)
1520 {
1521 return -EOPNOTSUPP;
1522 }
1523 #endif
1524
stmmac_test_l4filt_da_tcp(struct stmmac_priv * priv)1525 static int stmmac_test_l4filt_da_tcp(struct stmmac_priv *priv)
1526 {
1527 u16 dummy_port = 0x123;
1528
1529 return __stmmac_test_l4filt(priv, dummy_port, 0, ~0, 0, false);
1530 }
1531
stmmac_test_l4filt_sa_tcp(struct stmmac_priv * priv)1532 static int stmmac_test_l4filt_sa_tcp(struct stmmac_priv *priv)
1533 {
1534 u16 dummy_port = 0x123;
1535
1536 return __stmmac_test_l4filt(priv, 0, dummy_port, 0, ~0, false);
1537 }
1538
stmmac_test_l4filt_da_udp(struct stmmac_priv * priv)1539 static int stmmac_test_l4filt_da_udp(struct stmmac_priv *priv)
1540 {
1541 u16 dummy_port = 0x123;
1542
1543 return __stmmac_test_l4filt(priv, dummy_port, 0, ~0, 0, true);
1544 }
1545
stmmac_test_l4filt_sa_udp(struct stmmac_priv * priv)1546 static int stmmac_test_l4filt_sa_udp(struct stmmac_priv *priv)
1547 {
1548 u16 dummy_port = 0x123;
1549
1550 return __stmmac_test_l4filt(priv, 0, dummy_port, 0, ~0, true);
1551 }
1552
stmmac_test_arp_validate(struct sk_buff * skb,struct net_device * ndev,struct packet_type * pt,struct net_device * orig_ndev)1553 static int stmmac_test_arp_validate(struct sk_buff *skb,
1554 struct net_device *ndev,
1555 struct packet_type *pt,
1556 struct net_device *orig_ndev)
1557 {
1558 struct stmmac_test_priv *tpriv = pt->af_packet_priv;
1559 struct ethhdr *ehdr;
1560 struct arphdr *ahdr;
1561
1562 ehdr = (struct ethhdr *)skb_mac_header(skb);
1563 if (!ether_addr_equal_unaligned(ehdr->h_dest, tpriv->packet->src))
1564 goto out;
1565
1566 ahdr = arp_hdr(skb);
1567 if (ahdr->ar_op != htons(ARPOP_REPLY))
1568 goto out;
1569
1570 tpriv->ok = true;
1571 complete(&tpriv->comp);
1572 out:
1573 kfree_skb(skb);
1574 return 0;
1575 }
1576
stmmac_test_arpoffload(struct stmmac_priv * priv)1577 static int stmmac_test_arpoffload(struct stmmac_priv *priv)
1578 {
1579 unsigned char src[ETH_ALEN] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06};
1580 unsigned char dst[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
1581 struct stmmac_packet_attrs attr = { };
1582 struct stmmac_test_priv *tpriv;
1583 struct sk_buff *skb = NULL;
1584 u32 ip_addr = 0xdeadcafe;
1585 u32 ip_src = 0xdeadbeef;
1586 int ret;
1587
1588 if (!priv->dma_cap.arpoffsel)
1589 return -EOPNOTSUPP;
1590
1591 tpriv = kzalloc(sizeof(*tpriv), GFP_KERNEL);
1592 if (!tpriv)
1593 return -ENOMEM;
1594
1595 tpriv->ok = false;
1596 init_completion(&tpriv->comp);
1597
1598 tpriv->pt.type = htons(ETH_P_ARP);
1599 tpriv->pt.func = stmmac_test_arp_validate;
1600 tpriv->pt.dev = priv->dev;
1601 tpriv->pt.af_packet_priv = tpriv;
1602 tpriv->packet = &attr;
1603 dev_add_pack(&tpriv->pt);
1604
1605 attr.src = src;
1606 attr.ip_src = ip_src;
1607 attr.dst = dst;
1608 attr.ip_dst = ip_addr;
1609
1610 skb = stmmac_test_get_arp_skb(priv, &attr);
1611 if (!skb) {
1612 ret = -ENOMEM;
1613 goto cleanup;
1614 }
1615
1616 ret = stmmac_set_arp_offload(priv, priv->hw, true, ip_addr);
1617 if (ret)
1618 goto cleanup;
1619
1620 ret = dev_set_promiscuity(priv->dev, 1);
1621 if (ret)
1622 goto cleanup;
1623
1624 skb_set_queue_mapping(skb, 0);
1625 ret = dev_queue_xmit(skb);
1626 if (ret)
1627 goto cleanup_promisc;
1628
1629 wait_for_completion_timeout(&tpriv->comp, STMMAC_LB_TIMEOUT);
1630 ret = tpriv->ok ? 0 : -ETIMEDOUT;
1631
1632 cleanup_promisc:
1633 dev_set_promiscuity(priv->dev, -1);
1634 cleanup:
1635 stmmac_set_arp_offload(priv, priv->hw, false, 0x0);
1636 dev_remove_pack(&tpriv->pt);
1637 kfree(tpriv);
1638 return ret;
1639 }
1640
__stmmac_test_jumbo(struct stmmac_priv * priv,u16 queue)1641 static int __stmmac_test_jumbo(struct stmmac_priv *priv, u16 queue)
1642 {
1643 struct stmmac_packet_attrs attr = { };
1644 int size = priv->dma_buf_sz;
1645
1646 attr.dst = priv->dev->dev_addr;
1647 attr.max_size = size - ETH_FCS_LEN;
1648 attr.queue_mapping = queue;
1649
1650 return __stmmac_test_loopback(priv, &attr);
1651 }
1652
stmmac_test_jumbo(struct stmmac_priv * priv)1653 static int stmmac_test_jumbo(struct stmmac_priv *priv)
1654 {
1655 return __stmmac_test_jumbo(priv, 0);
1656 }
1657
stmmac_test_mjumbo(struct stmmac_priv * priv)1658 static int stmmac_test_mjumbo(struct stmmac_priv *priv)
1659 {
1660 u32 chan, tx_cnt = priv->plat->tx_queues_to_use;
1661 int ret;
1662
1663 if (tx_cnt <= 1)
1664 return -EOPNOTSUPP;
1665
1666 for (chan = 0; chan < tx_cnt; chan++) {
1667 ret = __stmmac_test_jumbo(priv, chan);
1668 if (ret)
1669 return ret;
1670 }
1671
1672 return 0;
1673 }
1674
stmmac_test_sph(struct stmmac_priv * priv)1675 static int stmmac_test_sph(struct stmmac_priv *priv)
1676 {
1677 unsigned long cnt_end, cnt_start = priv->xstats.rx_split_hdr_pkt_n;
1678 struct stmmac_packet_attrs attr = { };
1679 int ret;
1680
1681 if (!priv->sph)
1682 return -EOPNOTSUPP;
1683
1684 /* Check for UDP first */
1685 attr.dst = priv->dev->dev_addr;
1686 attr.tcp = false;
1687
1688 ret = __stmmac_test_loopback(priv, &attr);
1689 if (ret)
1690 return ret;
1691
1692 cnt_end = priv->xstats.rx_split_hdr_pkt_n;
1693 if (cnt_end <= cnt_start)
1694 return -EINVAL;
1695
1696 /* Check for TCP now */
1697 cnt_start = cnt_end;
1698
1699 attr.dst = priv->dev->dev_addr;
1700 attr.tcp = true;
1701
1702 ret = __stmmac_test_loopback(priv, &attr);
1703 if (ret)
1704 return ret;
1705
1706 cnt_end = priv->xstats.rx_split_hdr_pkt_n;
1707 if (cnt_end <= cnt_start)
1708 return -EINVAL;
1709
1710 return 0;
1711 }
1712
1713 #define STMMAC_LOOPBACK_NONE 0
1714 #define STMMAC_LOOPBACK_MAC 1
1715 #define STMMAC_LOOPBACK_PHY 2
1716
1717 static const struct stmmac_test {
1718 char name[ETH_GSTRING_LEN];
1719 int lb;
1720 int (*fn)(struct stmmac_priv *priv);
1721 } stmmac_selftests[] = {
1722 {
1723 .name = "MAC Loopback ",
1724 .lb = STMMAC_LOOPBACK_MAC,
1725 .fn = stmmac_test_mac_loopback,
1726 }, {
1727 .name = "PHY Loopback ",
1728 .lb = STMMAC_LOOPBACK_NONE, /* Test will handle it */
1729 .fn = stmmac_test_phy_loopback,
1730 }, {
1731 .name = "MMC Counters ",
1732 .lb = STMMAC_LOOPBACK_PHY,
1733 .fn = stmmac_test_mmc,
1734 }, {
1735 .name = "EEE ",
1736 .lb = STMMAC_LOOPBACK_PHY,
1737 .fn = stmmac_test_eee,
1738 }, {
1739 .name = "Hash Filter MC ",
1740 .lb = STMMAC_LOOPBACK_PHY,
1741 .fn = stmmac_test_hfilt,
1742 }, {
1743 .name = "Perfect Filter UC ",
1744 .lb = STMMAC_LOOPBACK_PHY,
1745 .fn = stmmac_test_pfilt,
1746 }, {
1747 .name = "MC Filter ",
1748 .lb = STMMAC_LOOPBACK_PHY,
1749 .fn = stmmac_test_mcfilt,
1750 }, {
1751 .name = "UC Filter ",
1752 .lb = STMMAC_LOOPBACK_PHY,
1753 .fn = stmmac_test_ucfilt,
1754 }, {
1755 .name = "Flow Control ",
1756 .lb = STMMAC_LOOPBACK_PHY,
1757 .fn = stmmac_test_flowctrl,
1758 }, {
1759 .name = "RSS ",
1760 .lb = STMMAC_LOOPBACK_PHY,
1761 .fn = stmmac_test_rss,
1762 }, {
1763 .name = "VLAN Filtering ",
1764 .lb = STMMAC_LOOPBACK_PHY,
1765 .fn = stmmac_test_vlanfilt,
1766 }, {
1767 .name = "Double VLAN Filtering",
1768 .lb = STMMAC_LOOPBACK_PHY,
1769 .fn = stmmac_test_dvlanfilt,
1770 }, {
1771 .name = "Flexible RX Parser ",
1772 .lb = STMMAC_LOOPBACK_PHY,
1773 .fn = stmmac_test_rxp,
1774 }, {
1775 .name = "SA Insertion (desc) ",
1776 .lb = STMMAC_LOOPBACK_PHY,
1777 .fn = stmmac_test_desc_sai,
1778 }, {
1779 .name = "SA Replacement (desc)",
1780 .lb = STMMAC_LOOPBACK_PHY,
1781 .fn = stmmac_test_desc_sar,
1782 }, {
1783 .name = "SA Insertion (reg) ",
1784 .lb = STMMAC_LOOPBACK_PHY,
1785 .fn = stmmac_test_reg_sai,
1786 }, {
1787 .name = "SA Replacement (reg)",
1788 .lb = STMMAC_LOOPBACK_PHY,
1789 .fn = stmmac_test_reg_sar,
1790 }, {
1791 .name = "VLAN TX Insertion ",
1792 .lb = STMMAC_LOOPBACK_PHY,
1793 .fn = stmmac_test_vlanoff,
1794 }, {
1795 .name = "SVLAN TX Insertion ",
1796 .lb = STMMAC_LOOPBACK_PHY,
1797 .fn = stmmac_test_svlanoff,
1798 }, {
1799 .name = "L3 DA Filtering ",
1800 .lb = STMMAC_LOOPBACK_PHY,
1801 .fn = stmmac_test_l3filt_da,
1802 }, {
1803 .name = "L3 SA Filtering ",
1804 .lb = STMMAC_LOOPBACK_PHY,
1805 .fn = stmmac_test_l3filt_sa,
1806 }, {
1807 .name = "L4 DA TCP Filtering ",
1808 .lb = STMMAC_LOOPBACK_PHY,
1809 .fn = stmmac_test_l4filt_da_tcp,
1810 }, {
1811 .name = "L4 SA TCP Filtering ",
1812 .lb = STMMAC_LOOPBACK_PHY,
1813 .fn = stmmac_test_l4filt_sa_tcp,
1814 }, {
1815 .name = "L4 DA UDP Filtering ",
1816 .lb = STMMAC_LOOPBACK_PHY,
1817 .fn = stmmac_test_l4filt_da_udp,
1818 }, {
1819 .name = "L4 SA UDP Filtering ",
1820 .lb = STMMAC_LOOPBACK_PHY,
1821 .fn = stmmac_test_l4filt_sa_udp,
1822 }, {
1823 .name = "ARP Offload ",
1824 .lb = STMMAC_LOOPBACK_PHY,
1825 .fn = stmmac_test_arpoffload,
1826 }, {
1827 .name = "Jumbo Frame ",
1828 .lb = STMMAC_LOOPBACK_PHY,
1829 .fn = stmmac_test_jumbo,
1830 }, {
1831 .name = "Multichannel Jumbo ",
1832 .lb = STMMAC_LOOPBACK_PHY,
1833 .fn = stmmac_test_mjumbo,
1834 }, {
1835 .name = "Split Header ",
1836 .lb = STMMAC_LOOPBACK_PHY,
1837 .fn = stmmac_test_sph,
1838 },
1839 };
1840
stmmac_selftest_run(struct net_device * dev,struct ethtool_test * etest,u64 * buf)1841 void stmmac_selftest_run(struct net_device *dev,
1842 struct ethtool_test *etest, u64 *buf)
1843 {
1844 struct stmmac_priv *priv = netdev_priv(dev);
1845 int count = stmmac_selftest_get_count(priv);
1846 int carrier = netif_carrier_ok(dev);
1847 int i, ret;
1848
1849 memset(buf, 0, sizeof(*buf) * count);
1850 stmmac_test_next_id = 0;
1851
1852 if (etest->flags != ETH_TEST_FL_OFFLINE) {
1853 netdev_err(priv->dev, "Only offline tests are supported\n");
1854 etest->flags |= ETH_TEST_FL_FAILED;
1855 return;
1856 } else if (!carrier) {
1857 netdev_err(priv->dev, "You need valid Link to execute tests\n");
1858 etest->flags |= ETH_TEST_FL_FAILED;
1859 return;
1860 }
1861
1862 /* We don't want extra traffic */
1863 netif_carrier_off(dev);
1864
1865 /* Wait for queues drain */
1866 msleep(200);
1867
1868 for (i = 0; i < count; i++) {
1869 ret = 0;
1870
1871 switch (stmmac_selftests[i].lb) {
1872 case STMMAC_LOOPBACK_PHY:
1873 ret = -EOPNOTSUPP;
1874 if (dev->phydev)
1875 ret = phy_loopback(dev->phydev, true);
1876 if (!ret)
1877 break;
1878 /* Fallthrough */
1879 case STMMAC_LOOPBACK_MAC:
1880 ret = stmmac_set_mac_loopback(priv, priv->ioaddr, true);
1881 break;
1882 case STMMAC_LOOPBACK_NONE:
1883 break;
1884 default:
1885 ret = -EOPNOTSUPP;
1886 break;
1887 }
1888
1889 /*
1890 * First tests will always be MAC / PHY loobpack. If any of
1891 * them is not supported we abort earlier.
1892 */
1893 if (ret) {
1894 netdev_err(priv->dev, "Loopback is not supported\n");
1895 etest->flags |= ETH_TEST_FL_FAILED;
1896 break;
1897 }
1898
1899 ret = stmmac_selftests[i].fn(priv);
1900 if (ret && (ret != -EOPNOTSUPP))
1901 etest->flags |= ETH_TEST_FL_FAILED;
1902 buf[i] = ret;
1903
1904 switch (stmmac_selftests[i].lb) {
1905 case STMMAC_LOOPBACK_PHY:
1906 ret = -EOPNOTSUPP;
1907 if (dev->phydev)
1908 ret = phy_loopback(dev->phydev, false);
1909 if (!ret)
1910 break;
1911 /* Fallthrough */
1912 case STMMAC_LOOPBACK_MAC:
1913 stmmac_set_mac_loopback(priv, priv->ioaddr, false);
1914 break;
1915 default:
1916 break;
1917 }
1918 }
1919
1920 /* Restart everything */
1921 if (carrier)
1922 netif_carrier_on(dev);
1923 }
1924
stmmac_selftest_get_strings(struct stmmac_priv * priv,u8 * data)1925 void stmmac_selftest_get_strings(struct stmmac_priv *priv, u8 *data)
1926 {
1927 u8 *p = data;
1928 int i;
1929
1930 for (i = 0; i < stmmac_selftest_get_count(priv); i++) {
1931 snprintf(p, ETH_GSTRING_LEN, "%2d. %s", i + 1,
1932 stmmac_selftests[i].name);
1933 p += ETH_GSTRING_LEN;
1934 }
1935 }
1936
stmmac_selftest_get_count(struct stmmac_priv * priv)1937 int stmmac_selftest_get_count(struct stmmac_priv *priv)
1938 {
1939 return ARRAY_SIZE(stmmac_selftests);
1940 }
1941