Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * Copyright (c) 2019 Synopsys, Inc. and/or its affiliates.
0004  * stmmac Selftests Support
0005  *
0006  * Author: Jose Abreu <joabreu@synopsys.com>
0007  *
0008  * Ported from stmmac by:
0009  * Copyright (C) 2021 Oleksij Rempel <o.rempel@pengutronix.de>
0010  */
0011 
0012 #include <linux/phy.h>
0013 #include <net/selftests.h>
0014 #include <net/tcp.h>
0015 #include <net/udp.h>
0016 
0017 struct net_packet_attrs {
0018     const unsigned char *src;
0019     const unsigned char *dst;
0020     u32 ip_src;
0021     u32 ip_dst;
0022     bool tcp;
0023     u16 sport;
0024     u16 dport;
0025     int timeout;
0026     int size;
0027     int max_size;
0028     u8 id;
0029     u16 queue_mapping;
0030 };
0031 
0032 struct net_test_priv {
0033     struct net_packet_attrs *packet;
0034     struct packet_type pt;
0035     struct completion comp;
0036     int double_vlan;
0037     int vlan_id;
0038     int ok;
0039 };
0040 
0041 struct netsfhdr {
0042     __be32 version;
0043     __be64 magic;
0044     u8 id;
0045 } __packed;
0046 
0047 static u8 net_test_next_id;
0048 
0049 #define NET_TEST_PKT_SIZE (sizeof(struct ethhdr) + sizeof(struct iphdr) + \
0050                sizeof(struct netsfhdr))
0051 #define NET_TEST_PKT_MAGIC  0xdeadcafecafedeadULL
0052 #define NET_LB_TIMEOUT      msecs_to_jiffies(200)
0053 
0054 static struct sk_buff *net_test_get_skb(struct net_device *ndev,
0055                     struct net_packet_attrs *attr)
0056 {
0057     struct sk_buff *skb = NULL;
0058     struct udphdr *uhdr = NULL;
0059     struct tcphdr *thdr = NULL;
0060     struct netsfhdr *shdr;
0061     struct ethhdr *ehdr;
0062     struct iphdr *ihdr;
0063     int iplen, size;
0064 
0065     size = attr->size + NET_TEST_PKT_SIZE;
0066 
0067     if (attr->tcp)
0068         size += sizeof(struct tcphdr);
0069     else
0070         size += sizeof(struct udphdr);
0071 
0072     if (attr->max_size && attr->max_size > size)
0073         size = attr->max_size;
0074 
0075     skb = netdev_alloc_skb(ndev, size);
0076     if (!skb)
0077         return NULL;
0078 
0079     prefetchw(skb->data);
0080 
0081     ehdr = skb_push(skb, ETH_HLEN);
0082     skb_reset_mac_header(skb);
0083 
0084     skb_set_network_header(skb, skb->len);
0085     ihdr = skb_put(skb, sizeof(*ihdr));
0086 
0087     skb_set_transport_header(skb, skb->len);
0088     if (attr->tcp)
0089         thdr = skb_put(skb, sizeof(*thdr));
0090     else
0091         uhdr = skb_put(skb, sizeof(*uhdr));
0092 
0093     eth_zero_addr(ehdr->h_dest);
0094 
0095     if (attr->src)
0096         ether_addr_copy(ehdr->h_source, attr->src);
0097     if (attr->dst)
0098         ether_addr_copy(ehdr->h_dest, attr->dst);
0099 
0100     ehdr->h_proto = htons(ETH_P_IP);
0101 
0102     if (attr->tcp) {
0103         thdr->source = htons(attr->sport);
0104         thdr->dest = htons(attr->dport);
0105         thdr->doff = sizeof(struct tcphdr) / 4;
0106         thdr->check = 0;
0107     } else {
0108         uhdr->source = htons(attr->sport);
0109         uhdr->dest = htons(attr->dport);
0110         uhdr->len = htons(sizeof(*shdr) + sizeof(*uhdr) + attr->size);
0111         if (attr->max_size)
0112             uhdr->len = htons(attr->max_size -
0113                       (sizeof(*ihdr) + sizeof(*ehdr)));
0114         uhdr->check = 0;
0115     }
0116 
0117     ihdr->ihl = 5;
0118     ihdr->ttl = 32;
0119     ihdr->version = 4;
0120     if (attr->tcp)
0121         ihdr->protocol = IPPROTO_TCP;
0122     else
0123         ihdr->protocol = IPPROTO_UDP;
0124     iplen = sizeof(*ihdr) + sizeof(*shdr) + attr->size;
0125     if (attr->tcp)
0126         iplen += sizeof(*thdr);
0127     else
0128         iplen += sizeof(*uhdr);
0129 
0130     if (attr->max_size)
0131         iplen = attr->max_size - sizeof(*ehdr);
0132 
0133     ihdr->tot_len = htons(iplen);
0134     ihdr->frag_off = 0;
0135     ihdr->saddr = htonl(attr->ip_src);
0136     ihdr->daddr = htonl(attr->ip_dst);
0137     ihdr->tos = 0;
0138     ihdr->id = 0;
0139     ip_send_check(ihdr);
0140 
0141     shdr = skb_put(skb, sizeof(*shdr));
0142     shdr->version = 0;
0143     shdr->magic = cpu_to_be64(NET_TEST_PKT_MAGIC);
0144     attr->id = net_test_next_id;
0145     shdr->id = net_test_next_id++;
0146 
0147     if (attr->size)
0148         skb_put(skb, attr->size);
0149     if (attr->max_size && attr->max_size > skb->len)
0150         skb_put(skb, attr->max_size - skb->len);
0151 
0152     skb->csum = 0;
0153     skb->ip_summed = CHECKSUM_PARTIAL;
0154     if (attr->tcp) {
0155         thdr->check = ~tcp_v4_check(skb->len, ihdr->saddr,
0156                         ihdr->daddr, 0);
0157         skb->csum_start = skb_transport_header(skb) - skb->head;
0158         skb->csum_offset = offsetof(struct tcphdr, check);
0159     } else {
0160         udp4_hwcsum(skb, ihdr->saddr, ihdr->daddr);
0161     }
0162 
0163     skb->protocol = htons(ETH_P_IP);
0164     skb->pkt_type = PACKET_HOST;
0165     skb->dev = ndev;
0166 
0167     return skb;
0168 }
0169 
0170 static int net_test_loopback_validate(struct sk_buff *skb,
0171                       struct net_device *ndev,
0172                       struct packet_type *pt,
0173                       struct net_device *orig_ndev)
0174 {
0175     struct net_test_priv *tpriv = pt->af_packet_priv;
0176     const unsigned char *src = tpriv->packet->src;
0177     const unsigned char *dst = tpriv->packet->dst;
0178     struct netsfhdr *shdr;
0179     struct ethhdr *ehdr;
0180     struct udphdr *uhdr;
0181     struct tcphdr *thdr;
0182     struct iphdr *ihdr;
0183 
0184     skb = skb_unshare(skb, GFP_ATOMIC);
0185     if (!skb)
0186         goto out;
0187 
0188     if (skb_linearize(skb))
0189         goto out;
0190     if (skb_headlen(skb) < (NET_TEST_PKT_SIZE - ETH_HLEN))
0191         goto out;
0192 
0193     ehdr = (struct ethhdr *)skb_mac_header(skb);
0194     if (dst) {
0195         if (!ether_addr_equal_unaligned(ehdr->h_dest, dst))
0196             goto out;
0197     }
0198 
0199     if (src) {
0200         if (!ether_addr_equal_unaligned(ehdr->h_source, src))
0201             goto out;
0202     }
0203 
0204     ihdr = ip_hdr(skb);
0205     if (tpriv->double_vlan)
0206         ihdr = (struct iphdr *)(skb_network_header(skb) + 4);
0207 
0208     if (tpriv->packet->tcp) {
0209         if (ihdr->protocol != IPPROTO_TCP)
0210             goto out;
0211 
0212         thdr = (struct tcphdr *)((u8 *)ihdr + 4 * ihdr->ihl);
0213         if (thdr->dest != htons(tpriv->packet->dport))
0214             goto out;
0215 
0216         shdr = (struct netsfhdr *)((u8 *)thdr + sizeof(*thdr));
0217     } else {
0218         if (ihdr->protocol != IPPROTO_UDP)
0219             goto out;
0220 
0221         uhdr = (struct udphdr *)((u8 *)ihdr + 4 * ihdr->ihl);
0222         if (uhdr->dest != htons(tpriv->packet->dport))
0223             goto out;
0224 
0225         shdr = (struct netsfhdr *)((u8 *)uhdr + sizeof(*uhdr));
0226     }
0227 
0228     if (shdr->magic != cpu_to_be64(NET_TEST_PKT_MAGIC))
0229         goto out;
0230     if (tpriv->packet->id != shdr->id)
0231         goto out;
0232 
0233     tpriv->ok = true;
0234     complete(&tpriv->comp);
0235 out:
0236     kfree_skb(skb);
0237     return 0;
0238 }
0239 
0240 static int __net_test_loopback(struct net_device *ndev,
0241                    struct net_packet_attrs *attr)
0242 {
0243     struct net_test_priv *tpriv;
0244     struct sk_buff *skb = NULL;
0245     int ret = 0;
0246 
0247     tpriv = kzalloc(sizeof(*tpriv), GFP_KERNEL);
0248     if (!tpriv)
0249         return -ENOMEM;
0250 
0251     tpriv->ok = false;
0252     init_completion(&tpriv->comp);
0253 
0254     tpriv->pt.type = htons(ETH_P_IP);
0255     tpriv->pt.func = net_test_loopback_validate;
0256     tpriv->pt.dev = ndev;
0257     tpriv->pt.af_packet_priv = tpriv;
0258     tpriv->packet = attr;
0259     dev_add_pack(&tpriv->pt);
0260 
0261     skb = net_test_get_skb(ndev, attr);
0262     if (!skb) {
0263         ret = -ENOMEM;
0264         goto cleanup;
0265     }
0266 
0267     ret = dev_direct_xmit(skb, attr->queue_mapping);
0268     if (ret < 0) {
0269         goto cleanup;
0270     } else if (ret > 0) {
0271         ret = -ENETUNREACH;
0272         goto cleanup;
0273     }
0274 
0275     if (!attr->timeout)
0276         attr->timeout = NET_LB_TIMEOUT;
0277 
0278     wait_for_completion_timeout(&tpriv->comp, attr->timeout);
0279     ret = tpriv->ok ? 0 : -ETIMEDOUT;
0280 
0281 cleanup:
0282     dev_remove_pack(&tpriv->pt);
0283     kfree(tpriv);
0284     return ret;
0285 }
0286 
0287 static int net_test_netif_carrier(struct net_device *ndev)
0288 {
0289     return netif_carrier_ok(ndev) ? 0 : -ENOLINK;
0290 }
0291 
0292 static int net_test_phy_phydev(struct net_device *ndev)
0293 {
0294     return ndev->phydev ? 0 : -EOPNOTSUPP;
0295 }
0296 
0297 static int net_test_phy_loopback_enable(struct net_device *ndev)
0298 {
0299     if (!ndev->phydev)
0300         return -EOPNOTSUPP;
0301 
0302     return phy_loopback(ndev->phydev, true);
0303 }
0304 
0305 static int net_test_phy_loopback_disable(struct net_device *ndev)
0306 {
0307     if (!ndev->phydev)
0308         return -EOPNOTSUPP;
0309 
0310     return phy_loopback(ndev->phydev, false);
0311 }
0312 
0313 static int net_test_phy_loopback_udp(struct net_device *ndev)
0314 {
0315     struct net_packet_attrs attr = { };
0316 
0317     attr.dst = ndev->dev_addr;
0318     return __net_test_loopback(ndev, &attr);
0319 }
0320 
0321 static int net_test_phy_loopback_udp_mtu(struct net_device *ndev)
0322 {
0323     struct net_packet_attrs attr = { };
0324 
0325     attr.dst = ndev->dev_addr;
0326     attr.max_size = ndev->mtu;
0327     return __net_test_loopback(ndev, &attr);
0328 }
0329 
0330 static int net_test_phy_loopback_tcp(struct net_device *ndev)
0331 {
0332     struct net_packet_attrs attr = { };
0333 
0334     attr.dst = ndev->dev_addr;
0335     attr.tcp = true;
0336     return __net_test_loopback(ndev, &attr);
0337 }
0338 
0339 static const struct net_test {
0340     char name[ETH_GSTRING_LEN];
0341     int (*fn)(struct net_device *ndev);
0342 } net_selftests[] = {
0343     {
0344         .name = "Carrier                       ",
0345         .fn = net_test_netif_carrier,
0346     }, {
0347         .name = "PHY dev is present            ",
0348         .fn = net_test_phy_phydev,
0349     }, {
0350         /* This test should be done before all PHY loopback test */
0351         .name = "PHY internal loopback, enable ",
0352         .fn = net_test_phy_loopback_enable,
0353     }, {
0354         .name = "PHY internal loopback, UDP    ",
0355         .fn = net_test_phy_loopback_udp,
0356     }, {
0357         .name = "PHY internal loopback, MTU    ",
0358         .fn = net_test_phy_loopback_udp_mtu,
0359     }, {
0360         .name = "PHY internal loopback, TCP    ",
0361         .fn = net_test_phy_loopback_tcp,
0362     }, {
0363         /* This test should be done after all PHY loopback test */
0364         .name = "PHY internal loopback, disable",
0365         .fn = net_test_phy_loopback_disable,
0366     },
0367 };
0368 
0369 void net_selftest(struct net_device *ndev, struct ethtool_test *etest, u64 *buf)
0370 {
0371     int count = net_selftest_get_count();
0372     int i;
0373 
0374     memset(buf, 0, sizeof(*buf) * count);
0375     net_test_next_id = 0;
0376 
0377     if (etest->flags != ETH_TEST_FL_OFFLINE) {
0378         netdev_err(ndev, "Only offline tests are supported\n");
0379         etest->flags |= ETH_TEST_FL_FAILED;
0380         return;
0381     }
0382 
0383 
0384     for (i = 0; i < count; i++) {
0385         buf[i] = net_selftests[i].fn(ndev);
0386         if (buf[i] && (buf[i] != -EOPNOTSUPP))
0387             etest->flags |= ETH_TEST_FL_FAILED;
0388     }
0389 }
0390 EXPORT_SYMBOL_GPL(net_selftest);
0391 
0392 int net_selftest_get_count(void)
0393 {
0394     return ARRAY_SIZE(net_selftests);
0395 }
0396 EXPORT_SYMBOL_GPL(net_selftest_get_count);
0397 
0398 void net_selftest_get_strings(u8 *data)
0399 {
0400     u8 *p = data;
0401     int i;
0402 
0403     for (i = 0; i < net_selftest_get_count(); i++) {
0404         snprintf(p, ETH_GSTRING_LEN, "%2d. %s", i + 1,
0405              net_selftests[i].name);
0406         p += ETH_GSTRING_LEN;
0407     }
0408 }
0409 EXPORT_SYMBOL_GPL(net_selftest_get_strings);
0410 
0411 MODULE_LICENSE("GPL v2");
0412 MODULE_AUTHOR("Oleksij Rempel <o.rempel@pengutronix.de>");
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!