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 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 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 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); 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_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 330 static 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 357 void 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 } 378 EXPORT_SYMBOL_GPL(net_selftest); 379 380 int net_selftest_get_count(void) 381 { 382 return ARRAY_SIZE(net_selftests); 383 } 384 EXPORT_SYMBOL_GPL(net_selftest_get_count); 385 386 void 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 } 397 EXPORT_SYMBOL_GPL(net_selftest_get_strings); 398 399 MODULE_LICENSE("GPL v2"); 400 MODULE_AUTHOR("Oleksij Rempel <o.rempel@pengutronix.de>"); 401