drivers/net/ethernet/stmicro/stmmac/stmmac_selftests.c

   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/tcp.h>
  15 #include <net/udp.h>
  16 #include "stmmac.h"
  17
  18 struct stmmachdr {
  19         __be32 version;
  20         __be64 magic;
  21         u8 id;
  22 } __packed;
  23
  24 #define STMMAC_TEST_PKT_SIZE (sizeof(struct ethhdr) + sizeof(struct iphdr) + \
  25                               sizeof(struct stmmachdr))
  26 #define STMMAC_TEST_PKT_MAGIC   0xdeadcafecafedeadULL
  27 #define STMMAC_LB_TIMEOUT       msecs_to_jiffies(200)
  28
  29 struct stmmac_packet_attrs {
  30         int vlan;
  31         int vlan_id_in;
  32         int vlan_id_out;
  33         unsigned char *src;
  34         unsigned char *dst;
  35         u32 ip_src;
  36         u32 ip_dst;
  37         int tcp;
  38         int sport;
  39         int dport;
  40         u32 exp_hash;
  41         int dont_wait;
  42         int timeout;
  43         int size;
  44         int remove_sa;
  45         u8 id;
  46 };
  47
  48 static u8 stmmac_test_next_id;
  49
  50 static struct sk_buff *stmmac_test_get_udp_skb(struct stmmac_priv *priv,
  51                                                struct stmmac_packet_attrs *attr)
  52 {
  53         struct sk_buff *skb = NULL;
  54         struct udphdr *uhdr = NULL;
  55         struct tcphdr *thdr = NULL;
  56         struct stmmachdr *shdr;
  57         struct ethhdr *ehdr;
  58         struct iphdr *ihdr;
  59         int iplen, size;
  60
  61         size = attr->size + STMMAC_TEST_PKT_SIZE;
  62         if (attr->vlan) {
  63                 size += 4;
  64                 if (attr->vlan > 1)
  65                         size += 4;
  66         }
  67
  68         if (attr->tcp)
  69                 size += sizeof(struct tcphdr);
  70         else
  71                 size += sizeof(struct udphdr);
  72
  73         skb = netdev_alloc_skb(priv->dev, size);
  74         if (!skb)
  75                 return NULL;
  76
  77         prefetchw(skb->data);
  78         skb_reserve(skb, NET_IP_ALIGN);
  79
  80         if (attr->vlan > 1)
  81                 ehdr = skb_push(skb, ETH_HLEN + 8);
  82         else if (attr->vlan)
  83                 ehdr = skb_push(skb, ETH_HLEN + 4);
  84         else if (attr->remove_sa)
  85                 ehdr = skb_push(skb, ETH_HLEN - 6);
  86         else
  87                 ehdr = skb_push(skb, ETH_HLEN);
  88         skb_reset_mac_header(skb);
  89
  90         skb_set_network_header(skb, skb->len);
  91         ihdr = skb_put(skb, sizeof(*ihdr));
  92
  93         skb_set_transport_header(skb, skb->len);
  94         if (attr->tcp)
  95                 thdr = skb_put(skb, sizeof(*thdr));
  96         else
  97                 uhdr = skb_put(skb, sizeof(*uhdr));
  98
  99         if (!attr->remove_sa)
 100                 eth_zero_addr(ehdr->h_source);
 101         eth_zero_addr(ehdr->h_dest);
 102         if (attr->src && !attr->remove_sa)
 103                 ether_addr_copy(ehdr->h_source, attr->src);
 104         if (attr->dst)
 105                 ether_addr_copy(ehdr->h_dest, attr->dst);
 106
 107         if (!attr->remove_sa) {
 108                 ehdr->h_proto = htons(ETH_P_IP);
 109         } else {
 110                 __be16 *ptr = (__be16 *)ehdr;
 111
 112                 /* HACK */
 113                 ptr[3] = htons(ETH_P_IP);
 114         }
 115
 116         if (attr->vlan) {
 117                 __be16 *tag, *proto;
 118
 119                 if (!attr->remove_sa) {
 120                         tag = (void *)ehdr + ETH_HLEN;
 121                         proto = (void *)ehdr + (2 * ETH_ALEN);
 122                 } else {
 123                         tag = (void *)ehdr + ETH_HLEN - 6;
 124                         proto = (void *)ehdr + ETH_ALEN;
 125                 }
 126
 127                 proto[0] = htons(ETH_P_8021Q);
 128                 tag[0] = htons(attr->vlan_id_out);
 129                 tag[1] = htons(ETH_P_IP);
 130                 if (attr->vlan > 1) {
 131                         proto[0] = htons(ETH_P_8021AD);
 132                         tag[1] = htons(ETH_P_8021Q);
 133                         tag[2] = htons(attr->vlan_id_in);
 134                         tag[3] = htons(ETH_P_IP);
 135                 }
 136         }
 137
 138         if (attr->tcp) {
 139                 thdr->source = htons(attr->sport);
 140                 thdr->dest = htons(attr->dport);
 141                 thdr->doff = sizeof(struct tcphdr) / 4;
 142                 thdr->check = 0;
 143         } else {
 144                 uhdr->source = htons(attr->sport);
 145                 uhdr->dest = htons(attr->dport);
 146                 uhdr->len = htons(sizeof(*shdr) + sizeof(*uhdr) + attr->size);
 147                 uhdr->check = 0;
 148         }
 149
 150         ihdr->ihl = 5;
 151         ihdr->ttl = 32;
 152         ihdr->version = 4;
 153         if (attr->tcp)
 154                 ihdr->protocol = IPPROTO_TCP;
 155         else
 156                 ihdr->protocol = IPPROTO_UDP;
 157         iplen = sizeof(*ihdr) + sizeof(*shdr) + attr->size;
 158         if (attr->tcp)
 159                 iplen += sizeof(*thdr);
 160         else
 161                 iplen += sizeof(*uhdr);
 162         ihdr->tot_len = htons(iplen);
 163         ihdr->frag_off = 0;
 164         ihdr->saddr = 0;
 165         ihdr->daddr = htonl(attr->ip_dst);
 166         ihdr->tos = 0;
 167         ihdr->id = 0;
 168         ip_send_check(ihdr);
 169
 170         shdr = skb_put(skb, sizeof(*shdr));
 171         shdr->version = 0;
 172         shdr->magic = cpu_to_be64(STMMAC_TEST_PKT_MAGIC);
 173         attr->id = stmmac_test_next_id;
 174         shdr->id = stmmac_test_next_id++;
 175
 176         if (attr->size)
 177                 skb_put(skb, attr->size);
 178
 179         skb->csum = 0;
 180         skb->ip_summed = CHECKSUM_PARTIAL;
 181         if (attr->tcp) {
 182                 thdr->check = ~tcp_v4_check(skb->len, ihdr->saddr, ihdr->daddr, 0);
 183                 skb->csum_start = skb_transport_header(skb) - skb->head;
 184                 skb->csum_offset = offsetof(struct tcphdr, check);
 185         } else {
 186                 udp4_hwcsum(skb, ihdr->saddr, ihdr->daddr);
 187         }
 188
 189         skb->protocol = htons(ETH_P_IP);
 190         skb->pkt_type = PACKET_HOST;
 191         skb->dev = priv->dev;
 192
 193         return skb;
 194 }
 195
 196 struct stmmac_test_priv {
 197         struct stmmac_packet_attrs *packet;
 198         struct packet_type pt;
 199         struct completion comp;
 200         int double_vlan;
 201         int vlan_id;
 202         int ok;
 203 };
 204
 205 static int stmmac_test_loopback_validate(struct sk_buff *skb,
 206                                          struct net_device *ndev,
 207                                          struct packet_type *pt,
 208                                          struct net_device *orig_ndev)
 209 {
 210         struct stmmac_test_priv *tpriv = pt->af_packet_priv;
 211         struct stmmachdr *shdr;
 212         struct ethhdr *ehdr;
 213         struct udphdr *uhdr;
 214         struct tcphdr *thdr;
 215         struct iphdr *ihdr;
 216
 217         skb = skb_unshare(skb, GFP_ATOMIC);
 218         if (!skb)
 219                 goto out;
 220
 221         if (skb_linearize(skb))
 222                 goto out;
 223         if (skb_headlen(skb) < (STMMAC_TEST_PKT_SIZE - ETH_HLEN))
 224                 goto out;
 225
 226         ehdr = (struct ethhdr *)skb_mac_header(skb);
 227         if (tpriv->packet->dst) {
 228                 if (!ether_addr_equal(ehdr->h_dest, tpriv->packet->dst))
 229                         goto out;
 230         }
 231         if (tpriv->packet->src) {
 232                 if (!ether_addr_equal(ehdr->h_source, orig_ndev->dev_addr))
 233                         goto out;
 234         }
 235
 236         ihdr = ip_hdr(skb);
 237         if (tpriv->double_vlan)
 238                 ihdr = (struct iphdr *)(skb_network_header(skb) + 4);
 239
 240         if (tpriv->packet->tcp) {
 241                 if (ihdr->protocol != IPPROTO_TCP)
 242                         goto out;
 243
 244                 thdr = (struct tcphdr *)((u8 *)ihdr + 4 * ihdr->ihl);
 245                 if (thdr->dest != htons(tpriv->packet->dport))
 246                         goto out;
 247
 248                 shdr = (struct stmmachdr *)((u8 *)thdr + sizeof(*thdr));
 249         } else {
 250                 if (ihdr->protocol != IPPROTO_UDP)
 251                         goto out;
 252
 253                 uhdr = (struct udphdr *)((u8 *)ihdr + 4 * ihdr->ihl);
 254                 if (uhdr->dest != htons(tpriv->packet->dport))
 255                         goto out;
 256
 257                 shdr = (struct stmmachdr *)((u8 *)uhdr + sizeof(*uhdr));
 258         }
 259
 260         if (shdr->magic != cpu_to_be64(STMMAC_TEST_PKT_MAGIC))
 261                 goto out;
 262         if (tpriv->packet->exp_hash && !skb->hash)
 263                 goto out;
 264         if (tpriv->packet->id != shdr->id)
 265                 goto out;
 266
 267         tpriv->ok = true;
 268         complete(&tpriv->comp);
 269 out:
 270         kfree_skb(skb);
 271         return 0;
 272 }
 273
 274 static int __stmmac_test_loopback(struct stmmac_priv *priv,
 275                                   struct stmmac_packet_attrs *attr)
 276 {
 277         struct stmmac_test_priv *tpriv;
 278         struct sk_buff *skb = NULL;
 279         int ret = 0;
 280
 281         tpriv = kzalloc(sizeof(*tpriv), GFP_KERNEL);
 282         if (!tpriv)
 283                 return -ENOMEM;
 284
 285         tpriv->ok = false;
 286         init_completion(&tpriv->comp);
 287
 288         tpriv->pt.type = htons(ETH_P_IP);
 289         tpriv->pt.func = stmmac_test_loopback_validate;
 290         tpriv->pt.dev = priv->dev;
 291         tpriv->pt.af_packet_priv = tpriv;
 292         tpriv->packet = attr;
 293         dev_add_pack(&tpriv->pt);
 294
 295         skb = stmmac_test_get_udp_skb(priv, attr);
 296         if (!skb) {
 297                 ret = -ENOMEM;
 298                 goto cleanup;
 299         }
 300
 301         skb_set_queue_mapping(skb, 0);
 302         ret = dev_queue_xmit(skb);
 303         if (ret)
 304                 goto cleanup;
 305
 306         if (attr->dont_wait)
 307                 goto cleanup;
 308
 309         if (!attr->timeout)
 310                 attr->timeout = STMMAC_LB_TIMEOUT;
 311
 312         wait_for_completion_timeout(&tpriv->comp, attr->timeout);
 313         ret = !tpriv->ok;
 314
 315 cleanup:
 316         dev_remove_pack(&tpriv->pt);
 317         kfree(tpriv);
 318         return ret;
 319 }
 320
 321 static int stmmac_test_mac_loopback(struct stmmac_priv *priv)
 322 {
 323         struct stmmac_packet_attrs attr = { };
 324
 325         attr.dst = priv->dev->dev_addr;
 326         return __stmmac_test_loopback(priv, &attr);
 327 }
 328
 329 static int stmmac_test_phy_loopback(struct stmmac_priv *priv)
 330 {
 331         struct stmmac_packet_attrs attr = { };
 332         int ret;
 333
 334         if (!priv->dev->phydev)
 335                 return -EBUSY;
 336
 337         ret = phy_loopback(priv->dev->phydev, true);
 338         if (ret)
 339                 return ret;
 340
 341         attr.dst = priv->dev->dev_addr;
 342         ret = __stmmac_test_loopback(priv, &attr);
 343
 344         phy_loopback(priv->dev->phydev, false);
 345         return ret;
 346 }
 347
 348 static int stmmac_test_mmc(struct stmmac_priv *priv)
 349 {
 350         struct stmmac_counters initial, final;
 351         int ret;
 352
 353         memset(&initial, 0, sizeof(initial));
 354         memset(&final, 0, sizeof(final));
 355
 356         if (!priv->dma_cap.rmon)
 357                 return -EOPNOTSUPP;
 358
 359         /* Save previous results into internal struct */
 360         stmmac_mmc_read(priv, priv->mmcaddr, &priv->mmc);
 361
 362         ret = stmmac_test_mac_loopback(priv);
 363         if (ret)
 364                 return ret;
 365
 366         /* These will be loopback results so no need to save them */
 367         stmmac_mmc_read(priv, priv->mmcaddr, &final);
 368
 369         /*
 370          * The number of MMC counters available depends on HW configuration
 371          * so we just use this one to validate the feature. I hope there is
 372          * not a version without this counter.
 373          */
 374         if (final.mmc_tx_framecount_g <= initial.mmc_tx_framecount_g)
 375                 return -EINVAL;
 376
 377         return 0;
 378 }
 379
 380 static int stmmac_test_eee(struct stmmac_priv *priv)
 381 {
 382         struct stmmac_extra_stats *initial, *final;
 383         int retries = 10;
 384         int ret;
 385
 386         if (!priv->dma_cap.eee || !priv->eee_active)
 387                 return -EOPNOTSUPP;
 388
 389         initial = kzalloc(sizeof(*initial), GFP_KERNEL);
 390         if (!initial)
 391                 return -ENOMEM;
 392
 393         final = kzalloc(sizeof(*final), GFP_KERNEL);
 394         if (!final) {
 395                 ret = -ENOMEM;
 396                 goto out_free_initial;
 397         }
 398
 399         memcpy(initial, &priv->xstats, sizeof(*initial));
 400
 401         ret = stmmac_test_mac_loopback(priv);
 402         if (ret)
 403                 goto out_free_final;
 404
 405         /* We have no traffic in the line so, sooner or later it will go LPI */
 406         while (--retries) {
 407                 memcpy(final, &priv->xstats, sizeof(*final));
 408
 409                 if (final->irq_tx_path_in_lpi_mode_n >
 410                     initial->irq_tx_path_in_lpi_mode_n)
 411                         break;
 412                 msleep(100);
 413         }
 414
 415         if (!retries) {
 416                 ret = -ETIMEDOUT;
 417                 goto out_free_final;
 418         }
 419
 420         if (final->irq_tx_path_in_lpi_mode_n <=
 421             initial->irq_tx_path_in_lpi_mode_n) {
 422                 ret = -EINVAL;
 423                 goto out_free_final;
 424         }
 425
 426         if (final->irq_tx_path_exit_lpi_mode_n <=
 427             initial->irq_tx_path_exit_lpi_mode_n) {
 428                 ret = -EINVAL;
 429                 goto out_free_final;
 430         }
 431
 432 out_free_final:
 433         kfree(final);
 434 out_free_initial:
 435         kfree(initial);
 436         return ret;
 437 }
 438
 439 static int stmmac_filter_check(struct stmmac_priv *priv)
 440 {
 441         if (!(priv->dev->flags & IFF_PROMISC))
 442                 return 0;
 443
 444         netdev_warn(priv->dev, "Test can't be run in promiscuous mode!\n");
 445         return -EOPNOTSUPP;
 446 }
 447
 448 static int stmmac_test_hfilt(struct stmmac_priv *priv)
 449 {
 450         unsigned char gd_addr[ETH_ALEN] = {0x01, 0x00, 0xcc, 0xcc, 0xdd, 0xdd};
 451         unsigned char bd_addr[ETH_ALEN] = {0x09, 0x00, 0xaa, 0xaa, 0xbb, 0xbb};
 452         struct stmmac_packet_attrs attr = { };
 453         int ret;
 454
 455         ret = stmmac_filter_check(priv);
 456         if (ret)
 457                 return ret;
 458
 459         ret = dev_mc_add(priv->dev, gd_addr);
 460         if (ret)
 461                 return ret;
 462
 463         attr.dst = gd_addr;
 464
 465         /* Shall receive packet */
 466         ret = __stmmac_test_loopback(priv, &attr);
 467         if (ret)
 468                 goto cleanup;
 469
 470         attr.dst = bd_addr;
 471
 472         /* Shall NOT receive packet */
 473         ret = __stmmac_test_loopback(priv, &attr);
 474         ret = !ret;
 475
 476 cleanup:
 477         dev_mc_del(priv->dev, gd_addr);
 478         return ret;
 479 }
 480
 481 static int stmmac_test_pfilt(struct stmmac_priv *priv)
 482 {
 483         unsigned char gd_addr[ETH_ALEN] = {0x00, 0x01, 0x44, 0x55, 0x66, 0x77};
 484         unsigned char bd_addr[ETH_ALEN] = {0x08, 0x00, 0x22, 0x33, 0x44, 0x55};
 485         struct stmmac_packet_attrs attr = { };
 486         int ret;
 487
 488         if (stmmac_filter_check(priv))
 489                 return -EOPNOTSUPP;
 490
 491         ret = dev_uc_add(priv->dev, gd_addr);
 492         if (ret)
 493                 return ret;
 494
 495         attr.dst = gd_addr;
 496
 497         /* Shall receive packet */
 498         ret = __stmmac_test_loopback(priv, &attr);
 499         if (ret)
 500                 goto cleanup;
 501
 502         attr.dst = bd_addr;
 503
 504         /* Shall NOT receive packet */
 505         ret = __stmmac_test_loopback(priv, &attr);
 506         ret = !ret;
 507
 508 cleanup:
 509         dev_uc_del(priv->dev, gd_addr);
 510         return ret;
 511 }
 512
 513 static int stmmac_dummy_sync(struct net_device *netdev, const u8 *addr)
 514 {
 515         return 0;
 516 }
 517
 518 static void stmmac_test_set_rx_mode(struct net_device *netdev)
 519 {
 520         /* As we are in test mode of ethtool we already own the rtnl lock
 521          * so no address will change from user. We can just call the
 522          * ndo_set_rx_mode() callback directly */
 523         if (netdev->netdev_ops->ndo_set_rx_mode)
 524                 netdev->netdev_ops->ndo_set_rx_mode(netdev);
 525 }
 526
 527 static int stmmac_test_mcfilt(struct stmmac_priv *priv)
 528 {
 529         unsigned char uc_addr[ETH_ALEN] = {0x00, 0x01, 0x44, 0x55, 0x66, 0x77};
 530         unsigned char mc_addr[ETH_ALEN] = {0x01, 0x01, 0x44, 0x55, 0x66, 0x77};
 531         struct stmmac_packet_attrs attr = { };
 532         int ret;
 533
 534         if (stmmac_filter_check(priv))
 535                 return -EOPNOTSUPP;
 536
 537         /* Remove all MC addresses */
 538         __dev_mc_unsync(priv->dev, NULL);
 539         stmmac_test_set_rx_mode(priv->dev);
 540
 541         ret = dev_uc_add(priv->dev, uc_addr);
 542         if (ret)
 543                 goto cleanup;
 544
 545         attr.dst = uc_addr;
 546
 547         /* Shall receive packet */
 548         ret = __stmmac_test_loopback(priv, &attr);
 549         if (ret)
 550                 goto cleanup;
 551
 552         attr.dst = mc_addr;
 553
 554         /* Shall NOT receive packet */
 555         ret = __stmmac_test_loopback(priv, &attr);
 556         ret = !ret;
 557
 558 cleanup:
 559         dev_uc_del(priv->dev, uc_addr);
 560         __dev_mc_sync(priv->dev, stmmac_dummy_sync, NULL);
 561         stmmac_test_set_rx_mode(priv->dev);
 562         return ret;
 563 }
 564
 565 static int stmmac_test_ucfilt(struct stmmac_priv *priv)
 566 {
 567         unsigned char uc_addr[ETH_ALEN] = {0x00, 0x01, 0x44, 0x55, 0x66, 0x77};
 568         unsigned char mc_addr[ETH_ALEN] = {0x01, 0x01, 0x44, 0x55, 0x66, 0x77};
 569         struct stmmac_packet_attrs attr = { };
 570         int ret;
 571
 572         if (stmmac_filter_check(priv))
 573                 return -EOPNOTSUPP;
 574
 575         /* Remove all UC addresses */
 576         __dev_uc_unsync(priv->dev, NULL);
 577         stmmac_test_set_rx_mode(priv->dev);
 578
 579         ret = dev_mc_add(priv->dev, mc_addr);
 580         if (ret)
 581                 goto cleanup;
 582
 583         attr.dst = mc_addr;
 584
 585         /* Shall receive packet */
 586         ret = __stmmac_test_loopback(priv, &attr);
 587         if (ret)
 588                 goto cleanup;
 589
 590         attr.dst = uc_addr;
 591
 592         /* Shall NOT receive packet */
 593         ret = __stmmac_test_loopback(priv, &attr);
 594         ret = !ret;
 595
 596 cleanup:
 597         dev_mc_del(priv->dev, mc_addr);
 598         __dev_uc_sync(priv->dev, stmmac_dummy_sync, NULL);
 599         stmmac_test_set_rx_mode(priv->dev);
 600         return ret;
 601 }
 602
 603 static int stmmac_test_flowctrl_validate(struct sk_buff *skb,
 604                                          struct net_device *ndev,
 605                                          struct packet_type *pt,
 606                                          struct net_device *orig_ndev)
 607 {
 608         struct stmmac_test_priv *tpriv = pt->af_packet_priv;
 609         struct ethhdr *ehdr;
 610
 611         ehdr = (struct ethhdr *)skb_mac_header(skb);
 612         if (!ether_addr_equal(ehdr->h_source, orig_ndev->dev_addr))
 613                 goto out;
 614         if (ehdr->h_proto != htons(ETH_P_PAUSE))
 615                 goto out;
 616
 617         tpriv->ok = true;
 618         complete(&tpriv->comp);
 619 out:
 620         kfree_skb(skb);
 621         return 0;
 622 }
 623
 624 static int stmmac_test_flowctrl(struct stmmac_priv *priv)
 625 {
 626         unsigned char paddr[ETH_ALEN] = {0x01, 0x80, 0xC2, 0x00, 0x00, 0x01};
 627         struct phy_device *phydev = priv->dev->phydev;
 628         u32 rx_cnt = priv->plat->rx_queues_to_use;
 629         struct stmmac_test_priv *tpriv;
 630         unsigned int pkt_count;
 631         int i, ret = 0;
 632
 633         if (!phydev || !phydev->pause)
 634                 return -EOPNOTSUPP;
 635
 636         tpriv = kzalloc(sizeof(*tpriv), GFP_KERNEL);
 637         if (!tpriv)
 638                 return -ENOMEM;
 639
 640         tpriv->ok = false;
 641         init_completion(&tpriv->comp);
 642         tpriv->pt.type = htons(ETH_P_PAUSE);
 643         tpriv->pt.func = stmmac_test_flowctrl_validate;
 644         tpriv->pt.dev = priv->dev;
 645         tpriv->pt.af_packet_priv = tpriv;
 646         dev_add_pack(&tpriv->pt);
 647
 648         /* Compute minimum number of packets to make FIFO full */
 649         pkt_count = priv->plat->rx_fifo_size;
 650         if (!pkt_count)
 651                 pkt_count = priv->dma_cap.rx_fifo_size;
 652         pkt_count /= 1400;
 653         pkt_count *= 2;
 654
 655         for (i = 0; i < rx_cnt; i++)
 656                 stmmac_stop_rx(priv, priv->ioaddr, i);
 657
 658         ret = dev_set_promiscuity(priv->dev, 1);
 659         if (ret)
 660                 goto cleanup;
 661
 662         ret = dev_mc_add(priv->dev, paddr);
 663         if (ret)
 664                 goto cleanup;
 665
 666         for (i = 0; i < pkt_count; i++) {
 667                 struct stmmac_packet_attrs attr = { };
 668
 669                 attr.dst = priv->dev->dev_addr;
 670                 attr.dont_wait = true;
 671                 attr.size = 1400;
 672
 673                 ret = __stmmac_test_loopback(priv, &attr);
 674                 if (ret)
 675                         goto cleanup;
 676                 if (tpriv->ok)
 677                         break;
 678         }
 679
 680         /* Wait for some time in case RX Watchdog is enabled */
 681         msleep(200);
 682
 683         for (i = 0; i < rx_cnt; i++) {
 684                 struct stmmac_channel *ch = &priv->channel[i];
 685
 686                 stmmac_start_rx(priv, priv->ioaddr, i);
 687                 local_bh_disable();
 688                 napi_reschedule(&ch->rx_napi);
 689                 local_bh_enable();
 690         }
 691
 692         wait_for_completion_timeout(&tpriv->comp, STMMAC_LB_TIMEOUT);
 693         ret = !tpriv->ok;
 694
 695 cleanup:
 696         dev_mc_del(priv->dev, paddr);
 697         dev_set_promiscuity(priv->dev, -1);
 698         dev_remove_pack(&tpriv->pt);
 699         kfree(tpriv);
 700         return ret;
 701 }
 702
 703 #define STMMAC_LOOPBACK_NONE    0
 704 #define STMMAC_LOOPBACK_MAC     1
 705 #define STMMAC_LOOPBACK_PHY     2
 706
 707 static const struct stmmac_test {
 708         char name[ETH_GSTRING_LEN];
 709         int lb;
 710         int (*fn)(struct stmmac_priv *priv);
 711 } stmmac_selftests[] = {
 712         {
 713                 .name = "MAC Loopback         ",
 714                 .lb = STMMAC_LOOPBACK_MAC,
 715                 .fn = stmmac_test_mac_loopback,
 716         }, {
 717                 .name = "PHY Loopback         ",
 718                 .lb = STMMAC_LOOPBACK_NONE, /* Test will handle it */
 719                 .fn = stmmac_test_phy_loopback,
 720         }, {
 721                 .name = "MMC Counters         ",
 722                 .lb = STMMAC_LOOPBACK_PHY,
 723                 .fn = stmmac_test_mmc,
 724         }, {
 725                 .name = "EEE                  ",
 726                 .lb = STMMAC_LOOPBACK_PHY,
 727                 .fn = stmmac_test_eee,
 728         }, {
 729                 .name = "Hash Filter MC       ",
 730                 .lb = STMMAC_LOOPBACK_PHY,
 731                 .fn = stmmac_test_hfilt,
 732         }, {
 733                 .name = "Perfect Filter UC    ",
 734                 .lb = STMMAC_LOOPBACK_PHY,
 735                 .fn = stmmac_test_pfilt,
 736         }, {
 737                 .name = "MC Filter            ",
 738                 .lb = STMMAC_LOOPBACK_PHY,
 739                 .fn = stmmac_test_mcfilt,
 740         }, {
 741                 .name = "UC Filter            ",
 742                 .lb = STMMAC_LOOPBACK_PHY,
 743                 .fn = stmmac_test_ucfilt,
 744         }, {
 745                 .name = "Flow Control         ",
 746                 .lb = STMMAC_LOOPBACK_PHY,
 747                 .fn = stmmac_test_flowctrl,
 748         },
 749 };
 750
 751 void stmmac_selftest_run(struct net_device *dev,
 752                          struct ethtool_test *etest, u64 *buf)
 753 {
 754         struct stmmac_priv *priv = netdev_priv(dev);
 755         int count = stmmac_selftest_get_count(priv);
 756         int carrier = netif_carrier_ok(dev);
 757         int i, ret;
 758
 759         memset(buf, 0, sizeof(*buf) * count);
 760         stmmac_test_next_id = 0;
 761
 762         if (etest->flags != ETH_TEST_FL_OFFLINE) {
 763                 netdev_err(priv->dev, "Only offline tests are supported\n");
 764                 etest->flags |= ETH_TEST_FL_FAILED;
 765                 return;
 766         } else if (!carrier) {
 767                 netdev_err(priv->dev, "You need valid Link to execute tests\n");
 768                 etest->flags |= ETH_TEST_FL_FAILED;
 769                 return;
 770         }
 771
 772         /* We don't want extra traffic */
 773         netif_carrier_off(dev);
 774
 775         /* Wait for queues drain */
 776         msleep(200);
 777
 778         for (i = 0; i < count; i++) {
 779                 ret = 0;
 780
 781                 switch (stmmac_selftests[i].lb) {
 782                 case STMMAC_LOOPBACK_PHY:
 783                         ret = -EOPNOTSUPP;
 784                         if (dev->phydev)
 785                                 ret = phy_loopback(dev->phydev, true);
 786                         if (!ret)
 787                                 break;
 788                         /* Fallthrough */
 789                 case STMMAC_LOOPBACK_MAC:
 790                         ret = stmmac_set_mac_loopback(priv, priv->ioaddr, true);
 791                         break;
 792                 case STMMAC_LOOPBACK_NONE:
 793                         break;
 794                 default:
 795                         ret = -EOPNOTSUPP;
 796                         break;
 797                 }
 798
 799                 /*
 800                  * First tests will always be MAC / PHY loobpack. If any of
 801                  * them is not supported we abort earlier.
 802                  */
 803                 if (ret) {
 804                         netdev_err(priv->dev, "Loopback is not supported\n");
 805                         etest->flags |= ETH_TEST_FL_FAILED;
 806                         break;
 807                 }
 808
 809                 ret = stmmac_selftests[i].fn(priv);
 810                 if (ret && (ret != -EOPNOTSUPP))
 811                         etest->flags |= ETH_TEST_FL_FAILED;
 812                 buf[i] = ret;
 813
 814                 switch (stmmac_selftests[i].lb) {
 815                 case STMMAC_LOOPBACK_PHY:
 816                         ret = -EOPNOTSUPP;
 817                         if (dev->phydev)
 818                                 ret = phy_loopback(dev->phydev, false);
 819                         if (!ret)
 820                                 break;
 821                         /* Fallthrough */
 822                 case STMMAC_LOOPBACK_MAC:
 823                         stmmac_set_mac_loopback(priv, priv->ioaddr, false);
 824                         break;
 825                 default:
 826                         break;
 827                 }
 828         }
 829
 830         /* Restart everything */
 831         if (carrier)
 832                 netif_carrier_on(dev);
 833 }
 834
 835 void stmmac_selftest_get_strings(struct stmmac_priv *priv, u8 *data)
 836 {
 837         u8 *p = data;
 838         int i;
 839
 840         for (i = 0; i < stmmac_selftest_get_count(priv); i++) {
 841                 snprintf(p, ETH_GSTRING_LEN, "%2d. %s", i + 1,
 842                          stmmac_selftests[i].name);
 843                 p += ETH_GSTRING_LEN;
 844         }
 845 }
 846
 847 int stmmac_selftest_get_count(struct stmmac_priv *priv)
 848 {
 849         return ARRAY_SIZE(stmmac_selftests);
 850 }