linux/net/core/selftests.c
<<
>>
Prefs
   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
  17struct net_packet_attrs {
  18        unsigned char *src;
  19        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
  32struct 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
  41struct netsfhdr {
  42        __be32 version;
  43        __be64 magic;
  44        u8 id;
  45} __packed;
  46
  47static 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
  54static 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
 170static 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        unsigned char *src = tpriv->packet->src;
 177        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);
 235out:
 236        kfree_skb(skb);
 237        return 0;
 238}
 239
 240static 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
 281cleanup:
 282        dev_remove_pack(&tpriv->pt);
 283        kfree(tpriv);
 284        return ret;
 285}
 286
 287static int net_test_netif_carrier(struct net_device *ndev)
 288{
 289        return netif_carrier_ok(ndev) ? 0 : -ENOLINK;
 290}
 291
 292static int net_test_phy_phydev(struct net_device *ndev)
 293{
 294        return ndev->phydev ? 0 : -EOPNOTSUPP;
 295}
 296
 297static 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
 305static 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
 313static 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
 321static int net_test_phy_loopback_tcp(struct net_device *ndev)
 322{
 323        struct net_packet_attrs attr = { };
 324
 325        attr.dst = ndev->dev_addr;
 326        attr.tcp = true;
 327        return __net_test_loopback(ndev, &attr);
 328}
 329
 330static const struct net_test {
 331        char name[ETH_GSTRING_LEN];
 332        int (*fn)(struct net_device *ndev);
 333} net_selftests[] = {
 334        {
 335                .name = "Carrier                       ",
 336                .fn = net_test_netif_carrier,
 337        }, {
 338                .name = "PHY dev is present            ",
 339                .fn = net_test_phy_phydev,
 340        }, {
 341                /* This test should be done before all PHY loopback test */
 342                .name = "PHY internal loopback, enable ",
 343                .fn = net_test_phy_loopback_enable,
 344        }, {
 345                .name = "PHY internal loopback, UDP    ",
 346                .fn = net_test_phy_loopback_udp,
 347        }, {
 348                .name = "PHY internal loopback, TCP    ",
 349                .fn = net_test_phy_loopback_tcp,
 350        }, {
 351                /* This test should be done after all PHY loopback test */
 352                .name = "PHY internal loopback, disable",
 353                .fn = net_test_phy_loopback_disable,
 354        },
 355};
 356
 357void net_selftest(struct net_device *ndev, struct ethtool_test *etest, u64 *buf)
 358{
 359        int count = net_selftest_get_count();
 360        int i;
 361
 362        memset(buf, 0, sizeof(*buf) * count);
 363        net_test_next_id = 0;
 364
 365        if (etest->flags != ETH_TEST_FL_OFFLINE) {
 366                netdev_err(ndev, "Only offline tests are supported\n");
 367                etest->flags |= ETH_TEST_FL_FAILED;
 368                return;
 369        }
 370
 371
 372        for (i = 0; i < count; i++) {
 373                buf[i] = net_selftests[i].fn(ndev);
 374                if (buf[i] && (buf[i] != -EOPNOTSUPP))
 375                        etest->flags |= ETH_TEST_FL_FAILED;
 376        }
 377}
 378EXPORT_SYMBOL_GPL(net_selftest);
 379
 380int net_selftest_get_count(void)
 381{
 382        return ARRAY_SIZE(net_selftests);
 383}
 384EXPORT_SYMBOL_GPL(net_selftest_get_count);
 385
 386void net_selftest_get_strings(u8 *data)
 387{
 388        u8 *p = data;
 389        int i;
 390
 391        for (i = 0; i < net_selftest_get_count(); i++) {
 392                snprintf(p, ETH_GSTRING_LEN, "%2d. %s", i + 1,
 393                         net_selftests[i].name);
 394                p += ETH_GSTRING_LEN;
 395        }
 396}
 397EXPORT_SYMBOL_GPL(net_selftest_get_strings);
 398
 399MODULE_LICENSE("GPL v2");
 400MODULE_AUTHOR("Oleksij Rempel <o.rempel@pengutronix.de>");
 401