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