net/core/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  * Ported from stmmac by:
   9  * Copyright (C) 2021 Oleksij Rempel <o.rempel@pengutronix.de>
  10  */
  11
  12 #include <linux/phy.h>
  13 #include <net/selftests.h>
  14 #include <net/tcp.h>
  15 #include <net/udp.h>
  16
  17 struct net_packet_attrs {
  18         const unsigned char *src;
  19         const unsigned char *dst;
  20         u32 ip_src;
  21         u32 ip_dst;
  22         bool tcp;
  23         u16 sport;
  24         u16 dport;
  25         int timeout;
  26         int size;
  27         int max_size;
  28         u8 id;
  29         u16 queue_mapping;
  30 };
  31
  32 struct net_test_priv {
  33         struct net_packet_attrs *packet;
  34         struct packet_type pt;
  35         struct completion comp;
  36         int double_vlan;
  37         int vlan_id;
  38         int ok;
  39 };
  40
  41 struct netsfhdr {
  42         __be32 version;
  43         __be64 magic;
  44         u8 id;
  45 } __packed;
  46
  47 static u8 net_test_next_id;
  48
  49 #define NET_TEST_PKT_SIZE (sizeof(struct ethhdr) + sizeof(struct iphdr) + \
  50                            sizeof(struct netsfhdr))
  51 #define NET_TEST_PKT_MAGIC      0xdeadcafecafedeadULL
  52 #define NET_LB_TIMEOUT          msecs_to_jiffies(200)
  53
  54 static struct sk_buff *net_test_get_skb(struct net_device *ndev,
  55                                         struct net_packet_attrs *attr)
  56 {
  57         struct sk_buff *skb = NULL;
  58         struct udphdr *uhdr = NULL;
  59         struct tcphdr *thdr = NULL;
  60         struct netsfhdr *shdr;
  61         struct ethhdr *ehdr;
  62         struct iphdr *ihdr;
  63         int iplen, size;
  64
  65         size = attr->size + NET_TEST_PKT_SIZE;
  66
  67         if (attr->tcp)
  68                 size += sizeof(struct tcphdr);
  69         else
  70                 size += sizeof(struct udphdr);
  71
  72         if (attr->max_size && attr->max_size > size)
  73                 size = attr->max_size;
  74
  75         skb = netdev_alloc_skb(ndev, size);
  76         if (!skb)
  77                 return NULL;
  78
  79         prefetchw(skb->data);
  80
  81         ehdr = skb_push(skb, ETH_HLEN);
  82         skb_reset_mac_header(skb);
  83
  84         skb_set_network_header(skb, skb->len);
  85         ihdr = skb_put(skb, sizeof(*ihdr));
  86
  87         skb_set_transport_header(skb, skb->len);
  88         if (attr->tcp)
  89                 thdr = skb_put(skb, sizeof(*thdr));
  90         else
  91                 uhdr = skb_put(skb, sizeof(*uhdr));
  92
  93         eth_zero_addr(ehdr->h_dest);
  94
  95         if (attr->src)
  96                 ether_addr_copy(ehdr->h_source, attr->src);
  97         if (attr->dst)
  98                 ether_addr_copy(ehdr->h_dest, attr->dst);
  99
 100         ehdr->h_proto = htons(ETH_P_IP);
 101
 102         if (attr->tcp) {
 103                 thdr->source = htons(attr->sport);
 104                 thdr->dest = htons(attr->dport);
 105                 thdr->doff = sizeof(struct tcphdr) / 4;
 106                 thdr->check = 0;
 107         } else {
 108                 uhdr->source = htons(attr->sport);
 109                 uhdr->dest = htons(attr->dport);
 110                 uhdr->len = htons(sizeof(*shdr) + sizeof(*uhdr) + attr->size);
 111                 if (attr->max_size)
 112                         uhdr->len = htons(attr->max_size -
 113                                           (sizeof(*ihdr) + sizeof(*ehdr)));
 114                 uhdr->check = 0;
 115         }
 116
 117         ihdr->ihl = 5;
 118         ihdr->ttl = 32;
 119         ihdr->version = 4;
 120         if (attr->tcp)
 121                 ihdr->protocol = IPPROTO_TCP;
 122         else
 123                 ihdr->protocol = IPPROTO_UDP;
 124         iplen = sizeof(*ihdr) + sizeof(*shdr) + attr->size;
 125         if (attr->tcp)
 126                 iplen += sizeof(*thdr);
 127         else
 128                 iplen += sizeof(*uhdr);
 129
 130         if (attr->max_size)
 131                 iplen = attr->max_size - sizeof(*ehdr);
 132
 133         ihdr->tot_len = htons(iplen);
 134         ihdr->frag_off = 0;
 135         ihdr->saddr = htonl(attr->ip_src);
 136         ihdr->daddr = htonl(attr->ip_dst);
 137         ihdr->tos = 0;
 138         ihdr->id = 0;
 139         ip_send_check(ihdr);
 140
 141         shdr = skb_put(skb, sizeof(*shdr));
 142         shdr->version = 0;
 143         shdr->magic = cpu_to_be64(NET_TEST_PKT_MAGIC);
 144         attr->id = net_test_next_id;
 145         shdr->id = net_test_next_id++;
 146
 147         if (attr->size)
 148                 skb_put(skb, attr->size);
 149         if (attr->max_size && attr->max_size > skb->len)
 150                 skb_put(skb, attr->max_size - skb->len);
 151
 152         skb->csum = 0;
 153         skb->ip_summed = CHECKSUM_PARTIAL;
 154         if (attr->tcp) {
 155                 thdr->check = ~tcp_v4_check(skb->len, ihdr->saddr,
 156                                             ihdr->daddr, 0);
 157                 skb->csum_start = skb_transport_header(skb) - skb->head;
 158                 skb->csum_offset = offsetof(struct tcphdr, check);
 159         } else {
 160                 udp4_hwcsum(skb, ihdr->saddr, ihdr->daddr);
 161         }
 162
 163         skb->protocol = htons(ETH_P_IP);
 164         skb->pkt_type = PACKET_HOST;
 165         skb->dev = ndev;
 166
 167         return skb;
 168 }
 169
 170 static int net_test_loopback_validate(struct sk_buff *skb,
 171                                       struct net_device *ndev,
 172                                       struct packet_type *pt,
 173                                       struct net_device *orig_ndev)
 174 {
 175         struct net_test_priv *tpriv = pt->af_packet_priv;
 176         const unsigned char *src = tpriv->packet->src;
 177         const unsigned char *dst = tpriv->packet->dst;
 178         struct netsfhdr *shdr;
 179         struct ethhdr *ehdr;
 180         struct udphdr *uhdr;
 181         struct tcphdr *thdr;
 182         struct iphdr *ihdr;
 183
 184         skb = skb_unshare(skb, GFP_ATOMIC);
 185         if (!skb)
 186                 goto out;
 187
 188         if (skb_linearize(skb))
 189                 goto out;
 190         if (skb_headlen(skb) < (NET_TEST_PKT_SIZE - ETH_HLEN))
 191                 goto out;
 192
 193         ehdr = (struct ethhdr *)skb_mac_header(skb);
 194         if (dst) {
 195                 if (!ether_addr_equal_unaligned(ehdr->h_dest, dst))
 196                         goto out;
 197         }
 198
 199         if (src) {
 200                 if (!ether_addr_equal_unaligned(ehdr->h_source, src))
 201                         goto out;
 202         }
 203
 204         ihdr = ip_hdr(skb);
 205         if (tpriv->double_vlan)
 206                 ihdr = (struct iphdr *)(skb_network_header(skb) + 4);
 207
 208         if (tpriv->packet->tcp) {
 209                 if (ihdr->protocol != IPPROTO_TCP)
 210                         goto out;
 211
 212                 thdr = (struct tcphdr *)((u8 *)ihdr + 4 * ihdr->ihl);
 213                 if (thdr->dest != htons(tpriv->packet->dport))
 214                         goto out;
 215
 216                 shdr = (struct netsfhdr *)((u8 *)thdr + sizeof(*thdr));
 217         } else {
 218                 if (ihdr->protocol != IPPROTO_UDP)
 219                         goto out;
 220
 221                 uhdr = (struct udphdr *)((u8 *)ihdr + 4 * ihdr->ihl);
 222                 if (uhdr->dest != htons(tpriv->packet->dport))
 223                         goto out;
 224
 225                 shdr = (struct netsfhdr *)((u8 *)uhdr + sizeof(*uhdr));
 226         }
 227
 228         if (shdr->magic != cpu_to_be64(NET_TEST_PKT_MAGIC))
 229                 goto out;
 230         if (tpriv->packet->id != shdr->id)
 231                 goto out;
 232
 233         tpriv->ok = true;
 234         complete(&tpriv->comp);
 235 out:
 236         kfree_skb(skb);
 237         return 0;
 238 }
 239
 240 static int __net_test_loopback(struct net_device *ndev,
 241                                struct net_packet_attrs *attr)
 242 {
 243         struct net_test_priv *tpriv;
 244         struct sk_buff *skb = NULL;
 245         int ret = 0;
 246
 247         tpriv = kzalloc(sizeof(*tpriv), GFP_KERNEL);
 248         if (!tpriv)
 249                 return -ENOMEM;
 250
 251         tpriv->ok = false;
 252         init_completion(&tpriv->comp);
 253
 254         tpriv->pt.type = htons(ETH_P_IP);
 255         tpriv->pt.func = net_test_loopback_validate;
 256         tpriv->pt.dev = ndev;
 257         tpriv->pt.af_packet_priv = tpriv;
 258         tpriv->packet = attr;
 259         dev_add_pack(&tpriv->pt);
 260
 261         skb = net_test_get_skb(ndev, attr);
 262         if (!skb) {
 263                 ret = -ENOMEM;
 264                 goto cleanup;
 265         }
 266
 267         ret = dev_direct_xmit(skb, attr->queue_mapping);
 268         if (ret < 0) {
 269                 goto cleanup;
 270         } else if (ret > 0) {
 271                 ret = -ENETUNREACH;
 272                 goto cleanup;
 273         }
 274
 275         if (!attr->timeout)
 276                 attr->timeout = NET_LB_TIMEOUT;
 277
 278         wait_for_completion_timeout(&tpriv->comp, attr->timeout);
 279         ret = tpriv->ok ? 0 : -ETIMEDOUT;
 280
 281 cleanup:
 282         dev_remove_pack(&tpriv->pt);
 283         kfree(tpriv);
 284         return ret;
 285 }
 286
 287 static int net_test_netif_carrier(struct net_device *ndev)
 288 {
 289         return netif_carrier_ok(ndev) ? 0 : -ENOLINK;
 290 }
 291
 292 static int net_test_phy_phydev(struct net_device *ndev)
 293 {
 294         return ndev->phydev ? 0 : -EOPNOTSUPP;
 295 }
 296
 297 static int net_test_phy_loopback_enable(struct net_device *ndev)
 298 {
 299         if (!ndev->phydev)
 300                 return -EOPNOTSUPP;
 301
 302         return phy_loopback(ndev->phydev, true);
 303 }
 304
 305 static int net_test_phy_loopback_disable(struct net_device *ndev)
 306 {
 307         if (!ndev->phydev)
 308                 return -EOPNOTSUPP;
 309
 310         return phy_loopback(ndev->phydev, false);
 311 }
 312
 313 static int net_test_phy_loopback_udp(struct net_device *ndev)
 314 {
 315         struct net_packet_attrs attr = { };
 316
 317         attr.dst = ndev->dev_addr;
 318         return __net_test_loopback(ndev, &attr);
 319 }
 320
 321 static int net_test_phy_loopback_udp_mtu(struct net_device *ndev)
 322 {
 323         struct net_packet_attrs attr = { };
 324
 325         attr.dst = ndev->dev_addr;
 326         attr.max_size = ndev->mtu;
 327         return __net_test_loopback(ndev, &attr);
 328 }
 329
 330 static int net_test_phy_loopback_tcp(struct net_device *ndev)
 331 {
 332         struct net_packet_attrs attr = { };
 333
 334         attr.dst = ndev->dev_addr;
 335         attr.tcp = true;
 336         return __net_test_loopback(ndev, &attr);
 337 }
 338
 339 static const struct net_test {
 340         char name[ETH_GSTRING_LEN];
 341         int (*fn)(struct net_device *ndev);
 342 } net_selftests[] = {
 343         {
 344                 .name = "Carrier                       ",
 345                 .fn = net_test_netif_carrier,
 346         }, {
 347                 .name = "PHY dev is present            ",
 348                 .fn = net_test_phy_phydev,
 349         }, {
 350                 /* This test should be done before all PHY loopback test */
 351                 .name = "PHY internal loopback, enable ",
 352                 .fn = net_test_phy_loopback_enable,
 353         }, {
 354                 .name = "PHY internal loopback, UDP    ",
 355                 .fn = net_test_phy_loopback_udp,
 356         }, {
 357                 .name = "PHY internal loopback, MTU    ",
 358                 .fn = net_test_phy_loopback_udp_mtu,
 359         }, {
 360                 .name = "PHY internal loopback, TCP    ",
 361                 .fn = net_test_phy_loopback_tcp,
 362         }, {
 363                 /* This test should be done after all PHY loopback test */
 364                 .name = "PHY internal loopback, disable",
 365                 .fn = net_test_phy_loopback_disable,
 366         },
 367 };
 368
 369 void net_selftest(struct net_device *ndev, struct ethtool_test *etest, u64 *buf)
 370 {
 371         int count = net_selftest_get_count();
 372         int i;
 373
 374         memset(buf, 0, sizeof(*buf) * count);
 375         net_test_next_id = 0;
 376
 377         if (etest->flags != ETH_TEST_FL_OFFLINE) {
 378                 netdev_err(ndev, "Only offline tests are supported\n");
 379                 etest->flags |= ETH_TEST_FL_FAILED;
 380                 return;
 381         }
 382
 383
 384         for (i = 0; i < count; i++) {
 385                 buf[i] = net_selftests[i].fn(ndev);
 386                 if (buf[i] && (buf[i] != -EOPNOTSUPP))
 387                         etest->flags |= ETH_TEST_FL_FAILED;
 388         }
 389 }
 390 EXPORT_SYMBOL_GPL(net_selftest);
 391
 392 int net_selftest_get_count(void)
 393 {
 394         return ARRAY_SIZE(net_selftests);
 395 }
 396 EXPORT_SYMBOL_GPL(net_selftest_get_count);
 397
 398 void net_selftest_get_strings(u8 *data)
 399 {
 400         int i;
 401
 402         for (i = 0; i < net_selftest_get_count(); i++)
 403                 ethtool_sprintf(&data, "%2d. %s", i + 1,
 404                                 net_selftests[i].name);
 405 }
 406 EXPORT_SYMBOL_GPL(net_selftest_get_strings);
 407
 408 MODULE_DESCRIPTION("Common library for generic PHY ethtool selftests");
 409 MODULE_LICENSE("GPL v2");
 410 MODULE_AUTHOR("Oleksij Rempel <o.rempel@pengutronix.de>");