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