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