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/completion.h> 10 #include <linux/ethtool.h> 11 #include <linux/ip.h> 12 #include <linux/phy.h> 13 #include <linux/udp.h> 14 #include <net/pkt_cls.h> 15 #include <net/tcp.h> 16 #include <net/udp.h> 17 #include <net/tc_act/tc_gact.h> 18 #include "stmmac.h" 19 20 struct stmmachdr { 21 __be32 version; 22 __be64 magic; 23 u8 id; 24 } __packed; 25 26 #define STMMAC_TEST_PKT_SIZE (sizeof(struct ethhdr) + sizeof(struct iphdr) + \ 27 sizeof(struct stmmachdr)) 28 #define STMMAC_TEST_PKT_MAGIC 0xdeadcafecafedeadULL 29 #define STMMAC_LB_TIMEOUT msecs_to_jiffies(200) 30 31 struct stmmac_packet_attrs { 32 int vlan; 33 int vlan_id_in; 34 int vlan_id_out; 35 unsigned char *src; 36 unsigned char *dst; 37 u32 ip_src; 38 u32 ip_dst; 39 int tcp; 40 int sport; 41 int dport; 42 u32 exp_hash; 43 int dont_wait; 44 int timeout; 45 int size; 46 int remove_sa; 47 u8 id; 48 }; 49 50 static u8 stmmac_test_next_id; 51 52 static struct sk_buff *stmmac_test_get_udp_skb(struct stmmac_priv *priv, 53 struct stmmac_packet_attrs *attr) 54 { 55 struct sk_buff *skb = NULL; 56 struct udphdr *uhdr = NULL; 57 struct tcphdr *thdr = NULL; 58 struct stmmachdr *shdr; 59 struct ethhdr *ehdr; 60 struct iphdr *ihdr; 61 int iplen, size; 62 63 size = attr->size + STMMAC_TEST_PKT_SIZE; 64 if (attr->vlan) { 65 size += 4; 66 if (attr->vlan > 1) 67 size += 4; 68 } 69 70 if (attr->tcp) 71 size += sizeof(struct tcphdr); 72 else 73 size += sizeof(struct udphdr); 74 75 skb = netdev_alloc_skb(priv->dev, size); 76 if (!skb) 77 return NULL; 78 79 prefetchw(skb->data); 80 skb_reserve(skb, NET_IP_ALIGN); 81 82 if (attr->vlan > 1) 83 ehdr = skb_push(skb, ETH_HLEN + 8); 84 else if (attr->vlan) 85 ehdr = skb_push(skb, ETH_HLEN + 4); 86 else if (attr->remove_sa) 87 ehdr = skb_push(skb, ETH_HLEN - 6); 88 else 89 ehdr = skb_push(skb, ETH_HLEN); 90 skb_reset_mac_header(skb); 91 92 skb_set_network_header(skb, skb->len); 93 ihdr = skb_put(skb, sizeof(*ihdr)); 94 95 skb_set_transport_header(skb, skb->len); 96 if (attr->tcp) 97 thdr = skb_put(skb, sizeof(*thdr)); 98 else 99 uhdr = skb_put(skb, sizeof(*uhdr)); 100 101 if (!attr->remove_sa) 102 eth_zero_addr(ehdr->h_source); 103 eth_zero_addr(ehdr->h_dest); 104 if (attr->src && !attr->remove_sa) 105 ether_addr_copy(ehdr->h_source, attr->src); 106 if (attr->dst) 107 ether_addr_copy(ehdr->h_dest, attr->dst); 108 109 if (!attr->remove_sa) { 110 ehdr->h_proto = htons(ETH_P_IP); 111 } else { 112 __be16 *ptr = (__be16 *)ehdr; 113 114 /* HACK */ 115 ptr[3] = htons(ETH_P_IP); 116 } 117 118 if (attr->vlan) { 119 __be16 *tag, *proto; 120 121 if (!attr->remove_sa) { 122 tag = (void *)ehdr + ETH_HLEN; 123 proto = (void *)ehdr + (2 * ETH_ALEN); 124 } else { 125 tag = (void *)ehdr + ETH_HLEN - 6; 126 proto = (void *)ehdr + ETH_ALEN; 127 } 128 129 proto[0] = htons(ETH_P_8021Q); 130 tag[0] = htons(attr->vlan_id_out); 131 tag[1] = htons(ETH_P_IP); 132 if (attr->vlan > 1) { 133 proto[0] = htons(ETH_P_8021AD); 134 tag[1] = htons(ETH_P_8021Q); 135 tag[2] = htons(attr->vlan_id_in); 136 tag[3] = htons(ETH_P_IP); 137 } 138 } 139 140 if (attr->tcp) { 141 thdr->source = htons(attr->sport); 142 thdr->dest = htons(attr->dport); 143 thdr->doff = sizeof(struct tcphdr) / 4; 144 thdr->check = 0; 145 } else { 146 uhdr->source = htons(attr->sport); 147 uhdr->dest = htons(attr->dport); 148 uhdr->len = htons(sizeof(*shdr) + sizeof(*uhdr) + attr->size); 149 uhdr->check = 0; 150 } 151 152 ihdr->ihl = 5; 153 ihdr->ttl = 32; 154 ihdr->version = 4; 155 if (attr->tcp) 156 ihdr->protocol = IPPROTO_TCP; 157 else 158 ihdr->protocol = IPPROTO_UDP; 159 iplen = sizeof(*ihdr) + sizeof(*shdr) + attr->size; 160 if (attr->tcp) 161 iplen += sizeof(*thdr); 162 else 163 iplen += sizeof(*uhdr); 164 ihdr->tot_len = htons(iplen); 165 ihdr->frag_off = 0; 166 ihdr->saddr = 0; 167 ihdr->daddr = htonl(attr->ip_dst); 168 ihdr->tos = 0; 169 ihdr->id = 0; 170 ip_send_check(ihdr); 171 172 shdr = skb_put(skb, sizeof(*shdr)); 173 shdr->version = 0; 174 shdr->magic = cpu_to_be64(STMMAC_TEST_PKT_MAGIC); 175 attr->id = stmmac_test_next_id; 176 shdr->id = stmmac_test_next_id++; 177 178 if (attr->size) 179 skb_put(skb, attr->size); 180 181 skb->csum = 0; 182 skb->ip_summed = CHECKSUM_PARTIAL; 183 if (attr->tcp) { 184 thdr->check = ~tcp_v4_check(skb->len, ihdr->saddr, ihdr->daddr, 0); 185 skb->csum_start = skb_transport_header(skb) - skb->head; 186 skb->csum_offset = offsetof(struct tcphdr, check); 187 } else { 188 udp4_hwcsum(skb, ihdr->saddr, ihdr->daddr); 189 } 190 191 skb->protocol = htons(ETH_P_IP); 192 skb->pkt_type = PACKET_HOST; 193 skb->dev = priv->dev; 194 195 return skb; 196 } 197 198 struct stmmac_test_priv { 199 struct stmmac_packet_attrs *packet; 200 struct packet_type pt; 201 struct completion comp; 202 int double_vlan; 203 int vlan_id; 204 int ok; 205 }; 206 207 static int stmmac_test_loopback_validate(struct sk_buff *skb, 208 struct net_device *ndev, 209 struct packet_type *pt, 210 struct net_device *orig_ndev) 211 { 212 struct stmmac_test_priv *tpriv = pt->af_packet_priv; 213 struct stmmachdr *shdr; 214 struct ethhdr *ehdr; 215 struct udphdr *uhdr; 216 struct tcphdr *thdr; 217 struct iphdr *ihdr; 218 219 skb = skb_unshare(skb, GFP_ATOMIC); 220 if (!skb) 221 goto out; 222 223 if (skb_linearize(skb)) 224 goto out; 225 if (skb_headlen(skb) < (STMMAC_TEST_PKT_SIZE - ETH_HLEN)) 226 goto out; 227 228 ehdr = (struct ethhdr *)skb_mac_header(skb); 229 if (tpriv->packet->dst) { 230 if (!ether_addr_equal(ehdr->h_dest, tpriv->packet->dst)) 231 goto out; 232 } 233 if (tpriv->packet->src) { 234 if (!ether_addr_equal(ehdr->h_source, tpriv->packet->src)) 235 goto out; 236 } 237 238 ihdr = ip_hdr(skb); 239 if (tpriv->double_vlan) 240 ihdr = (struct iphdr *)(skb_network_header(skb) + 4); 241 242 if (tpriv->packet->tcp) { 243 if (ihdr->protocol != IPPROTO_TCP) 244 goto out; 245 246 thdr = (struct tcphdr *)((u8 *)ihdr + 4 * ihdr->ihl); 247 if (thdr->dest != htons(tpriv->packet->dport)) 248 goto out; 249 250 shdr = (struct stmmachdr *)((u8 *)thdr + sizeof(*thdr)); 251 } else { 252 if (ihdr->protocol != IPPROTO_UDP) 253 goto out; 254 255 uhdr = (struct udphdr *)((u8 *)ihdr + 4 * ihdr->ihl); 256 if (uhdr->dest != htons(tpriv->packet->dport)) 257 goto out; 258 259 shdr = (struct stmmachdr *)((u8 *)uhdr + sizeof(*uhdr)); 260 } 261 262 if (shdr->magic != cpu_to_be64(STMMAC_TEST_PKT_MAGIC)) 263 goto out; 264 if (tpriv->packet->exp_hash && !skb->hash) 265 goto out; 266 if (tpriv->packet->id != shdr->id) 267 goto out; 268 269 tpriv->ok = true; 270 complete(&tpriv->comp); 271 out: 272 kfree_skb(skb); 273 return 0; 274 } 275 276 static int __stmmac_test_loopback(struct stmmac_priv *priv, 277 struct stmmac_packet_attrs *attr) 278 { 279 struct stmmac_test_priv *tpriv; 280 struct sk_buff *skb = NULL; 281 int ret = 0; 282 283 tpriv = kzalloc(sizeof(*tpriv), GFP_KERNEL); 284 if (!tpriv) 285 return -ENOMEM; 286 287 tpriv->ok = false; 288 init_completion(&tpriv->comp); 289 290 tpriv->pt.type = htons(ETH_P_IP); 291 tpriv->pt.func = stmmac_test_loopback_validate; 292 tpriv->pt.dev = priv->dev; 293 tpriv->pt.af_packet_priv = tpriv; 294 tpriv->packet = attr; 295 dev_add_pack(&tpriv->pt); 296 297 skb = stmmac_test_get_udp_skb(priv, attr); 298 if (!skb) { 299 ret = -ENOMEM; 300 goto cleanup; 301 } 302 303 skb_set_queue_mapping(skb, 0); 304 ret = dev_queue_xmit(skb); 305 if (ret) 306 goto cleanup; 307 308 if (attr->dont_wait) 309 goto cleanup; 310 311 if (!attr->timeout) 312 attr->timeout = STMMAC_LB_TIMEOUT; 313 314 wait_for_completion_timeout(&tpriv->comp, attr->timeout); 315 ret = !tpriv->ok; 316 317 cleanup: 318 dev_remove_pack(&tpriv->pt); 319 kfree(tpriv); 320 return ret; 321 } 322 323 static int stmmac_test_mac_loopback(struct stmmac_priv *priv) 324 { 325 struct stmmac_packet_attrs attr = { }; 326 327 attr.dst = priv->dev->dev_addr; 328 return __stmmac_test_loopback(priv, &attr); 329 } 330 331 static int stmmac_test_phy_loopback(struct stmmac_priv *priv) 332 { 333 struct stmmac_packet_attrs attr = { }; 334 int ret; 335 336 if (!priv->dev->phydev) 337 return -EBUSY; 338 339 ret = phy_loopback(priv->dev->phydev, true); 340 if (ret) 341 return ret; 342 343 attr.dst = priv->dev->dev_addr; 344 ret = __stmmac_test_loopback(priv, &attr); 345 346 phy_loopback(priv->dev->phydev, false); 347 return ret; 348 } 349 350 static int stmmac_test_mmc(struct stmmac_priv *priv) 351 { 352 struct stmmac_counters initial, final; 353 int ret; 354 355 memset(&initial, 0, sizeof(initial)); 356 memset(&final, 0, sizeof(final)); 357 358 if (!priv->dma_cap.rmon) 359 return -EOPNOTSUPP; 360 361 /* Save previous results into internal struct */ 362 stmmac_mmc_read(priv, priv->mmcaddr, &priv->mmc); 363 364 ret = stmmac_test_mac_loopback(priv); 365 if (ret) 366 return ret; 367 368 /* These will be loopback results so no need to save them */ 369 stmmac_mmc_read(priv, priv->mmcaddr, &final); 370 371 /* 372 * The number of MMC counters available depends on HW configuration 373 * so we just use this one to validate the feature. I hope there is 374 * not a version without this counter. 375 */ 376 if (final.mmc_tx_framecount_g <= initial.mmc_tx_framecount_g) 377 return -EINVAL; 378 379 return 0; 380 } 381 382 static int stmmac_test_eee(struct stmmac_priv *priv) 383 { 384 struct stmmac_extra_stats *initial, *final; 385 int retries = 10; 386 int ret; 387 388 if (!priv->dma_cap.eee || !priv->eee_active) 389 return -EOPNOTSUPP; 390 391 initial = kzalloc(sizeof(*initial), GFP_KERNEL); 392 if (!initial) 393 return -ENOMEM; 394 395 final = kzalloc(sizeof(*final), GFP_KERNEL); 396 if (!final) { 397 ret = -ENOMEM; 398 goto out_free_initial; 399 } 400 401 memcpy(initial, &priv->xstats, sizeof(*initial)); 402 403 ret = stmmac_test_mac_loopback(priv); 404 if (ret) 405 goto out_free_final; 406 407 /* We have no traffic in the line so, sooner or later it will go LPI */ 408 while (--retries) { 409 memcpy(final, &priv->xstats, sizeof(*final)); 410 411 if (final->irq_tx_path_in_lpi_mode_n > 412 initial->irq_tx_path_in_lpi_mode_n) 413 break; 414 msleep(100); 415 } 416 417 if (!retries) { 418 ret = -ETIMEDOUT; 419 goto out_free_final; 420 } 421 422 if (final->irq_tx_path_in_lpi_mode_n <= 423 initial->irq_tx_path_in_lpi_mode_n) { 424 ret = -EINVAL; 425 goto out_free_final; 426 } 427 428 if (final->irq_tx_path_exit_lpi_mode_n <= 429 initial->irq_tx_path_exit_lpi_mode_n) { 430 ret = -EINVAL; 431 goto out_free_final; 432 } 433 434 out_free_final: 435 kfree(final); 436 out_free_initial: 437 kfree(initial); 438 return ret; 439 } 440 441 static int stmmac_filter_check(struct stmmac_priv *priv) 442 { 443 if (!(priv->dev->flags & IFF_PROMISC)) 444 return 0; 445 446 netdev_warn(priv->dev, "Test can't be run in promiscuous mode!\n"); 447 return -EOPNOTSUPP; 448 } 449 450 static int stmmac_test_hfilt(struct stmmac_priv *priv) 451 { 452 unsigned char gd_addr[ETH_ALEN] = {0x01, 0x00, 0xcc, 0xcc, 0xdd, 0xdd}; 453 unsigned char bd_addr[ETH_ALEN] = {0x09, 0x00, 0xaa, 0xaa, 0xbb, 0xbb}; 454 struct stmmac_packet_attrs attr = { }; 455 int ret; 456 457 ret = stmmac_filter_check(priv); 458 if (ret) 459 return ret; 460 461 ret = dev_mc_add(priv->dev, gd_addr); 462 if (ret) 463 return ret; 464 465 attr.dst = gd_addr; 466 467 /* Shall receive packet */ 468 ret = __stmmac_test_loopback(priv, &attr); 469 if (ret) 470 goto cleanup; 471 472 attr.dst = bd_addr; 473 474 /* Shall NOT receive packet */ 475 ret = __stmmac_test_loopback(priv, &attr); 476 ret = !ret; 477 478 cleanup: 479 dev_mc_del(priv->dev, gd_addr); 480 return ret; 481 } 482 483 static int stmmac_test_pfilt(struct stmmac_priv *priv) 484 { 485 unsigned char gd_addr[ETH_ALEN] = {0x00, 0x01, 0x44, 0x55, 0x66, 0x77}; 486 unsigned char bd_addr[ETH_ALEN] = {0x08, 0x00, 0x22, 0x33, 0x44, 0x55}; 487 struct stmmac_packet_attrs attr = { }; 488 int ret; 489 490 if (stmmac_filter_check(priv)) 491 return -EOPNOTSUPP; 492 493 ret = dev_uc_add(priv->dev, gd_addr); 494 if (ret) 495 return ret; 496 497 attr.dst = gd_addr; 498 499 /* Shall receive packet */ 500 ret = __stmmac_test_loopback(priv, &attr); 501 if (ret) 502 goto cleanup; 503 504 attr.dst = bd_addr; 505 506 /* Shall NOT receive packet */ 507 ret = __stmmac_test_loopback(priv, &attr); 508 ret = !ret; 509 510 cleanup: 511 dev_uc_del(priv->dev, gd_addr); 512 return ret; 513 } 514 515 static int stmmac_dummy_sync(struct net_device *netdev, const u8 *addr) 516 { 517 return 0; 518 } 519 520 static void stmmac_test_set_rx_mode(struct net_device *netdev) 521 { 522 /* As we are in test mode of ethtool we already own the rtnl lock 523 * so no address will change from user. We can just call the 524 * ndo_set_rx_mode() callback directly */ 525 if (netdev->netdev_ops->ndo_set_rx_mode) 526 netdev->netdev_ops->ndo_set_rx_mode(netdev); 527 } 528 529 static int stmmac_test_mcfilt(struct stmmac_priv *priv) 530 { 531 unsigned char uc_addr[ETH_ALEN] = {0x00, 0x01, 0x44, 0x55, 0x66, 0x77}; 532 unsigned char mc_addr[ETH_ALEN] = {0x01, 0x01, 0x44, 0x55, 0x66, 0x77}; 533 struct stmmac_packet_attrs attr = { }; 534 int ret; 535 536 if (stmmac_filter_check(priv)) 537 return -EOPNOTSUPP; 538 539 /* Remove all MC addresses */ 540 __dev_mc_unsync(priv->dev, NULL); 541 stmmac_test_set_rx_mode(priv->dev); 542 543 ret = dev_uc_add(priv->dev, uc_addr); 544 if (ret) 545 goto cleanup; 546 547 attr.dst = uc_addr; 548 549 /* Shall receive packet */ 550 ret = __stmmac_test_loopback(priv, &attr); 551 if (ret) 552 goto cleanup; 553 554 attr.dst = mc_addr; 555 556 /* Shall NOT receive packet */ 557 ret = __stmmac_test_loopback(priv, &attr); 558 ret = !ret; 559 560 cleanup: 561 dev_uc_del(priv->dev, uc_addr); 562 __dev_mc_sync(priv->dev, stmmac_dummy_sync, NULL); 563 stmmac_test_set_rx_mode(priv->dev); 564 return ret; 565 } 566 567 static int stmmac_test_ucfilt(struct stmmac_priv *priv) 568 { 569 unsigned char uc_addr[ETH_ALEN] = {0x00, 0x01, 0x44, 0x55, 0x66, 0x77}; 570 unsigned char mc_addr[ETH_ALEN] = {0x01, 0x01, 0x44, 0x55, 0x66, 0x77}; 571 struct stmmac_packet_attrs attr = { }; 572 int ret; 573 574 if (stmmac_filter_check(priv)) 575 return -EOPNOTSUPP; 576 577 /* Remove all UC addresses */ 578 __dev_uc_unsync(priv->dev, NULL); 579 stmmac_test_set_rx_mode(priv->dev); 580 581 ret = dev_mc_add(priv->dev, mc_addr); 582 if (ret) 583 goto cleanup; 584 585 attr.dst = mc_addr; 586 587 /* Shall receive packet */ 588 ret = __stmmac_test_loopback(priv, &attr); 589 if (ret) 590 goto cleanup; 591 592 attr.dst = uc_addr; 593 594 /* Shall NOT receive packet */ 595 ret = __stmmac_test_loopback(priv, &attr); 596 ret = !ret; 597 598 cleanup: 599 dev_mc_del(priv->dev, mc_addr); 600 __dev_uc_sync(priv->dev, stmmac_dummy_sync, NULL); 601 stmmac_test_set_rx_mode(priv->dev); 602 return ret; 603 } 604 605 static int stmmac_test_flowctrl_validate(struct sk_buff *skb, 606 struct net_device *ndev, 607 struct packet_type *pt, 608 struct net_device *orig_ndev) 609 { 610 struct stmmac_test_priv *tpriv = pt->af_packet_priv; 611 struct ethhdr *ehdr; 612 613 ehdr = (struct ethhdr *)skb_mac_header(skb); 614 if (!ether_addr_equal(ehdr->h_source, orig_ndev->dev_addr)) 615 goto out; 616 if (ehdr->h_proto != htons(ETH_P_PAUSE)) 617 goto out; 618 619 tpriv->ok = true; 620 complete(&tpriv->comp); 621 out: 622 kfree_skb(skb); 623 return 0; 624 } 625 626 static int stmmac_test_flowctrl(struct stmmac_priv *priv) 627 { 628 unsigned char paddr[ETH_ALEN] = {0x01, 0x80, 0xC2, 0x00, 0x00, 0x01}; 629 struct phy_device *phydev = priv->dev->phydev; 630 u32 rx_cnt = priv->plat->rx_queues_to_use; 631 struct stmmac_test_priv *tpriv; 632 unsigned int pkt_count; 633 int i, ret = 0; 634 635 if (!phydev || !phydev->pause) 636 return -EOPNOTSUPP; 637 638 tpriv = kzalloc(sizeof(*tpriv), GFP_KERNEL); 639 if (!tpriv) 640 return -ENOMEM; 641 642 tpriv->ok = false; 643 init_completion(&tpriv->comp); 644 tpriv->pt.type = htons(ETH_P_PAUSE); 645 tpriv->pt.func = stmmac_test_flowctrl_validate; 646 tpriv->pt.dev = priv->dev; 647 tpriv->pt.af_packet_priv = tpriv; 648 dev_add_pack(&tpriv->pt); 649 650 /* Compute minimum number of packets to make FIFO full */ 651 pkt_count = priv->plat->rx_fifo_size; 652 if (!pkt_count) 653 pkt_count = priv->dma_cap.rx_fifo_size; 654 pkt_count /= 1400; 655 pkt_count *= 2; 656 657 for (i = 0; i < rx_cnt; i++) 658 stmmac_stop_rx(priv, priv->ioaddr, i); 659 660 ret = dev_set_promiscuity(priv->dev, 1); 661 if (ret) 662 goto cleanup; 663 664 ret = dev_mc_add(priv->dev, paddr); 665 if (ret) 666 goto cleanup; 667 668 for (i = 0; i < pkt_count; i++) { 669 struct stmmac_packet_attrs attr = { }; 670 671 attr.dst = priv->dev->dev_addr; 672 attr.dont_wait = true; 673 attr.size = 1400; 674 675 ret = __stmmac_test_loopback(priv, &attr); 676 if (ret) 677 goto cleanup; 678 if (tpriv->ok) 679 break; 680 } 681 682 /* Wait for some time in case RX Watchdog is enabled */ 683 msleep(200); 684 685 for (i = 0; i < rx_cnt; i++) { 686 struct stmmac_channel *ch = &priv->channel[i]; 687 688 stmmac_start_rx(priv, priv->ioaddr, i); 689 local_bh_disable(); 690 napi_reschedule(&ch->rx_napi); 691 local_bh_enable(); 692 } 693 694 wait_for_completion_timeout(&tpriv->comp, STMMAC_LB_TIMEOUT); 695 ret = !tpriv->ok; 696 697 cleanup: 698 dev_mc_del(priv->dev, paddr); 699 dev_set_promiscuity(priv->dev, -1); 700 dev_remove_pack(&tpriv->pt); 701 kfree(tpriv); 702 return ret; 703 } 704 705 static int stmmac_test_rss(struct stmmac_priv *priv) 706 { 707 struct stmmac_packet_attrs attr = { }; 708 709 if (!priv->dma_cap.rssen || !priv->rss.enable) 710 return -EOPNOTSUPP; 711 712 attr.dst = priv->dev->dev_addr; 713 attr.exp_hash = true; 714 attr.sport = 0x321; 715 attr.dport = 0x123; 716 717 return __stmmac_test_loopback(priv, &attr); 718 } 719 720 static int stmmac_test_vlan_validate(struct sk_buff *skb, 721 struct net_device *ndev, 722 struct packet_type *pt, 723 struct net_device *orig_ndev) 724 { 725 struct stmmac_test_priv *tpriv = pt->af_packet_priv; 726 struct stmmachdr *shdr; 727 struct ethhdr *ehdr; 728 struct udphdr *uhdr; 729 struct iphdr *ihdr; 730 731 skb = skb_unshare(skb, GFP_ATOMIC); 732 if (!skb) 733 goto out; 734 735 if (skb_linearize(skb)) 736 goto out; 737 if (skb_headlen(skb) < (STMMAC_TEST_PKT_SIZE - ETH_HLEN)) 738 goto out; 739 740 ehdr = (struct ethhdr *)skb_mac_header(skb); 741 if (!ether_addr_equal(ehdr->h_dest, tpriv->packet->dst)) 742 goto out; 743 744 ihdr = ip_hdr(skb); 745 if (tpriv->double_vlan) 746 ihdr = (struct iphdr *)(skb_network_header(skb) + 4); 747 if (ihdr->protocol != IPPROTO_UDP) 748 goto out; 749 750 uhdr = (struct udphdr *)((u8 *)ihdr + 4 * ihdr->ihl); 751 if (uhdr->dest != htons(tpriv->packet->dport)) 752 goto out; 753 754 shdr = (struct stmmachdr *)((u8 *)uhdr + sizeof(*uhdr)); 755 if (shdr->magic != cpu_to_be64(STMMAC_TEST_PKT_MAGIC)) 756 goto out; 757 758 tpriv->ok = true; 759 complete(&tpriv->comp); 760 761 out: 762 kfree_skb(skb); 763 return 0; 764 } 765 766 static int stmmac_test_vlanfilt(struct stmmac_priv *priv) 767 { 768 struct stmmac_packet_attrs attr = { }; 769 struct stmmac_test_priv *tpriv; 770 struct sk_buff *skb = NULL; 771 int ret = 0, i; 772 773 if (!priv->dma_cap.vlhash) 774 return -EOPNOTSUPP; 775 776 tpriv = kzalloc(sizeof(*tpriv), GFP_KERNEL); 777 if (!tpriv) 778 return -ENOMEM; 779 780 tpriv->ok = false; 781 init_completion(&tpriv->comp); 782 783 tpriv->pt.type = htons(ETH_P_IP); 784 tpriv->pt.func = stmmac_test_vlan_validate; 785 tpriv->pt.dev = priv->dev; 786 tpriv->pt.af_packet_priv = tpriv; 787 tpriv->packet = &attr; 788 789 /* 790 * As we use HASH filtering, false positives may appear. This is a 791 * specially chosen ID so that adjacent IDs (+4) have different 792 * HASH values. 793 */ 794 tpriv->vlan_id = 0x123; 795 dev_add_pack(&tpriv->pt); 796 797 ret = vlan_vid_add(priv->dev, htons(ETH_P_8021Q), tpriv->vlan_id); 798 if (ret) 799 goto cleanup; 800 801 for (i = 0; i < 4; i++) { 802 attr.vlan = 1; 803 attr.vlan_id_out = tpriv->vlan_id + i; 804 attr.dst = priv->dev->dev_addr; 805 attr.sport = 9; 806 attr.dport = 9; 807 808 skb = stmmac_test_get_udp_skb(priv, &attr); 809 if (!skb) { 810 ret = -ENOMEM; 811 goto vlan_del; 812 } 813 814 skb_set_queue_mapping(skb, 0); 815 ret = dev_queue_xmit(skb); 816 if (ret) 817 goto vlan_del; 818 819 wait_for_completion_timeout(&tpriv->comp, STMMAC_LB_TIMEOUT); 820 ret = !tpriv->ok; 821 if (ret && !i) { 822 goto vlan_del; 823 } else if (!ret && i) { 824 ret = -1; 825 goto vlan_del; 826 } else { 827 ret = 0; 828 } 829 830 tpriv->ok = false; 831 } 832 833 vlan_del: 834 vlan_vid_del(priv->dev, htons(ETH_P_8021Q), tpriv->vlan_id); 835 cleanup: 836 dev_remove_pack(&tpriv->pt); 837 kfree(tpriv); 838 return ret; 839 } 840 841 static int stmmac_test_dvlanfilt(struct stmmac_priv *priv) 842 { 843 struct stmmac_packet_attrs attr = { }; 844 struct stmmac_test_priv *tpriv; 845 struct sk_buff *skb = NULL; 846 int ret = 0, i; 847 848 if (!priv->dma_cap.vlhash) 849 return -EOPNOTSUPP; 850 851 tpriv = kzalloc(sizeof(*tpriv), GFP_KERNEL); 852 if (!tpriv) 853 return -ENOMEM; 854 855 tpriv->ok = false; 856 tpriv->double_vlan = true; 857 init_completion(&tpriv->comp); 858 859 tpriv->pt.type = htons(ETH_P_8021Q); 860 tpriv->pt.func = stmmac_test_vlan_validate; 861 tpriv->pt.dev = priv->dev; 862 tpriv->pt.af_packet_priv = tpriv; 863 tpriv->packet = &attr; 864 865 /* 866 * As we use HASH filtering, false positives may appear. This is a 867 * specially chosen ID so that adjacent IDs (+4) have different 868 * HASH values. 869 */ 870 tpriv->vlan_id = 0x123; 871 dev_add_pack(&tpriv->pt); 872 873 ret = vlan_vid_add(priv->dev, htons(ETH_P_8021AD), tpriv->vlan_id); 874 if (ret) 875 goto cleanup; 876 877 for (i = 0; i < 4; i++) { 878 attr.vlan = 2; 879 attr.vlan_id_out = tpriv->vlan_id + i; 880 attr.dst = priv->dev->dev_addr; 881 attr.sport = 9; 882 attr.dport = 9; 883 884 skb = stmmac_test_get_udp_skb(priv, &attr); 885 if (!skb) { 886 ret = -ENOMEM; 887 goto vlan_del; 888 } 889 890 skb_set_queue_mapping(skb, 0); 891 ret = dev_queue_xmit(skb); 892 if (ret) 893 goto vlan_del; 894 895 wait_for_completion_timeout(&tpriv->comp, STMMAC_LB_TIMEOUT); 896 ret = !tpriv->ok; 897 if (ret && !i) { 898 goto vlan_del; 899 } else if (!ret && i) { 900 ret = -1; 901 goto vlan_del; 902 } else { 903 ret = 0; 904 } 905 906 tpriv->ok = false; 907 } 908 909 vlan_del: 910 vlan_vid_del(priv->dev, htons(ETH_P_8021AD), tpriv->vlan_id); 911 cleanup: 912 dev_remove_pack(&tpriv->pt); 913 kfree(tpriv); 914 return ret; 915 } 916 917 #ifdef CONFIG_NET_CLS_ACT 918 static int stmmac_test_rxp(struct stmmac_priv *priv) 919 { 920 unsigned char addr[ETH_ALEN] = {0xde, 0xad, 0xbe, 0xef, 0x00, 0x00}; 921 struct tc_cls_u32_offload cls_u32 = { }; 922 struct stmmac_packet_attrs attr = { }; 923 struct tc_action **actions, *act; 924 struct tc_u32_sel *sel; 925 struct tcf_exts *exts; 926 int ret, i, nk = 1; 927 928 if (!tc_can_offload(priv->dev)) 929 return -EOPNOTSUPP; 930 if (!priv->dma_cap.frpsel) 931 return -EOPNOTSUPP; 932 933 sel = kzalloc(sizeof(*sel) + nk * sizeof(struct tc_u32_key), GFP_KERNEL); 934 if (!sel) 935 return -ENOMEM; 936 937 exts = kzalloc(sizeof(*exts), GFP_KERNEL); 938 if (!exts) { 939 ret = -ENOMEM; 940 goto cleanup_sel; 941 } 942 943 actions = kzalloc(nk * sizeof(*actions), GFP_KERNEL); 944 if (!actions) { 945 ret = -ENOMEM; 946 goto cleanup_exts; 947 } 948 949 act = kzalloc(nk * sizeof(*act), GFP_KERNEL); 950 if (!act) { 951 ret = -ENOMEM; 952 goto cleanup_actions; 953 } 954 955 cls_u32.command = TC_CLSU32_NEW_KNODE; 956 cls_u32.common.chain_index = 0; 957 cls_u32.common.protocol = htons(ETH_P_ALL); 958 cls_u32.knode.exts = exts; 959 cls_u32.knode.sel = sel; 960 cls_u32.knode.handle = 0x123; 961 962 exts->nr_actions = nk; 963 exts->actions = actions; 964 for (i = 0; i < nk; i++) { 965 struct tcf_gact *gact = to_gact(&act[i]); 966 967 actions[i] = &act[i]; 968 gact->tcf_action = TC_ACT_SHOT; 969 } 970 971 sel->nkeys = nk; 972 sel->offshift = 0; 973 sel->keys[0].off = 6; 974 sel->keys[0].val = htonl(0xdeadbeef); 975 sel->keys[0].mask = ~0x0; 976 977 ret = stmmac_tc_setup_cls_u32(priv, priv, &cls_u32); 978 if (ret) 979 goto cleanup_act; 980 981 attr.dst = priv->dev->dev_addr; 982 attr.src = addr; 983 984 ret = __stmmac_test_loopback(priv, &attr); 985 ret = !ret; /* Shall NOT receive packet */ 986 987 cls_u32.command = TC_CLSU32_DELETE_KNODE; 988 stmmac_tc_setup_cls_u32(priv, priv, &cls_u32); 989 990 cleanup_act: 991 kfree(act); 992 cleanup_actions: 993 kfree(actions); 994 cleanup_exts: 995 kfree(exts); 996 cleanup_sel: 997 kfree(sel); 998 return ret; 999 } 1000 #else 1001 static int stmmac_test_rxp(struct stmmac_priv *priv) 1002 { 1003 return -EOPNOTSUPP; 1004 } 1005 #endif 1006 1007 #define STMMAC_LOOPBACK_NONE 0 1008 #define STMMAC_LOOPBACK_MAC 1 1009 #define STMMAC_LOOPBACK_PHY 2 1010 1011 static const struct stmmac_test { 1012 char name[ETH_GSTRING_LEN]; 1013 int lb; 1014 int (*fn)(struct stmmac_priv *priv); 1015 } stmmac_selftests[] = { 1016 { 1017 .name = "MAC Loopback ", 1018 .lb = STMMAC_LOOPBACK_MAC, 1019 .fn = stmmac_test_mac_loopback, 1020 }, { 1021 .name = "PHY Loopback ", 1022 .lb = STMMAC_LOOPBACK_NONE, /* Test will handle it */ 1023 .fn = stmmac_test_phy_loopback, 1024 }, { 1025 .name = "MMC Counters ", 1026 .lb = STMMAC_LOOPBACK_PHY, 1027 .fn = stmmac_test_mmc, 1028 }, { 1029 .name = "EEE ", 1030 .lb = STMMAC_LOOPBACK_PHY, 1031 .fn = stmmac_test_eee, 1032 }, { 1033 .name = "Hash Filter MC ", 1034 .lb = STMMAC_LOOPBACK_PHY, 1035 .fn = stmmac_test_hfilt, 1036 }, { 1037 .name = "Perfect Filter UC ", 1038 .lb = STMMAC_LOOPBACK_PHY, 1039 .fn = stmmac_test_pfilt, 1040 }, { 1041 .name = "MC Filter ", 1042 .lb = STMMAC_LOOPBACK_PHY, 1043 .fn = stmmac_test_mcfilt, 1044 }, { 1045 .name = "UC Filter ", 1046 .lb = STMMAC_LOOPBACK_PHY, 1047 .fn = stmmac_test_ucfilt, 1048 }, { 1049 .name = "Flow Control ", 1050 .lb = STMMAC_LOOPBACK_PHY, 1051 .fn = stmmac_test_flowctrl, 1052 }, { 1053 .name = "RSS ", 1054 .lb = STMMAC_LOOPBACK_PHY, 1055 .fn = stmmac_test_rss, 1056 }, { 1057 .name = "VLAN Filtering ", 1058 .lb = STMMAC_LOOPBACK_PHY, 1059 .fn = stmmac_test_vlanfilt, 1060 }, { 1061 .name = "Double VLAN Filtering", 1062 .lb = STMMAC_LOOPBACK_PHY, 1063 .fn = stmmac_test_dvlanfilt, 1064 }, { 1065 .name = "Flexible RX Parser ", 1066 .lb = STMMAC_LOOPBACK_PHY, 1067 .fn = stmmac_test_rxp, 1068 }, 1069 }; 1070 1071 void stmmac_selftest_run(struct net_device *dev, 1072 struct ethtool_test *etest, u64 *buf) 1073 { 1074 struct stmmac_priv *priv = netdev_priv(dev); 1075 int count = stmmac_selftest_get_count(priv); 1076 int carrier = netif_carrier_ok(dev); 1077 int i, ret; 1078 1079 memset(buf, 0, sizeof(*buf) * count); 1080 stmmac_test_next_id = 0; 1081 1082 if (etest->flags != ETH_TEST_FL_OFFLINE) { 1083 netdev_err(priv->dev, "Only offline tests are supported\n"); 1084 etest->flags |= ETH_TEST_FL_FAILED; 1085 return; 1086 } else if (!carrier) { 1087 netdev_err(priv->dev, "You need valid Link to execute tests\n"); 1088 etest->flags |= ETH_TEST_FL_FAILED; 1089 return; 1090 } 1091 1092 /* We don't want extra traffic */ 1093 netif_carrier_off(dev); 1094 1095 /* Wait for queues drain */ 1096 msleep(200); 1097 1098 for (i = 0; i < count; i++) { 1099 ret = 0; 1100 1101 switch (stmmac_selftests[i].lb) { 1102 case STMMAC_LOOPBACK_PHY: 1103 ret = -EOPNOTSUPP; 1104 if (dev->phydev) 1105 ret = phy_loopback(dev->phydev, true); 1106 if (!ret) 1107 break; 1108 /* Fallthrough */ 1109 case STMMAC_LOOPBACK_MAC: 1110 ret = stmmac_set_mac_loopback(priv, priv->ioaddr, true); 1111 break; 1112 case STMMAC_LOOPBACK_NONE: 1113 break; 1114 default: 1115 ret = -EOPNOTSUPP; 1116 break; 1117 } 1118 1119 /* 1120 * First tests will always be MAC / PHY loobpack. If any of 1121 * them is not supported we abort earlier. 1122 */ 1123 if (ret) { 1124 netdev_err(priv->dev, "Loopback is not supported\n"); 1125 etest->flags |= ETH_TEST_FL_FAILED; 1126 break; 1127 } 1128 1129 ret = stmmac_selftests[i].fn(priv); 1130 if (ret && (ret != -EOPNOTSUPP)) 1131 etest->flags |= ETH_TEST_FL_FAILED; 1132 buf[i] = ret; 1133 1134 switch (stmmac_selftests[i].lb) { 1135 case STMMAC_LOOPBACK_PHY: 1136 ret = -EOPNOTSUPP; 1137 if (dev->phydev) 1138 ret = phy_loopback(dev->phydev, false); 1139 if (!ret) 1140 break; 1141 /* Fallthrough */ 1142 case STMMAC_LOOPBACK_MAC: 1143 stmmac_set_mac_loopback(priv, priv->ioaddr, false); 1144 break; 1145 default: 1146 break; 1147 } 1148 } 1149 1150 /* Restart everything */ 1151 if (carrier) 1152 netif_carrier_on(dev); 1153 } 1154 1155 void stmmac_selftest_get_strings(struct stmmac_priv *priv, u8 *data) 1156 { 1157 u8 *p = data; 1158 int i; 1159 1160 for (i = 0; i < stmmac_selftest_get_count(priv); i++) { 1161 snprintf(p, ETH_GSTRING_LEN, "%2d. %s", i + 1, 1162 stmmac_selftests[i].name); 1163 p += ETH_GSTRING_LEN; 1164 } 1165 } 1166 1167 int stmmac_selftest_get_count(struct stmmac_priv *priv) 1168 { 1169 return ARRAY_SIZE(stmmac_selftests); 1170 } 1171