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 bool bad_csum; 31 }; 32 33 struct net_test_priv { 34 struct net_packet_attrs *packet; 35 struct packet_type pt; 36 struct completion comp; 37 int double_vlan; 38 int vlan_id; 39 int ok; 40 }; 41 42 struct netsfhdr { 43 __be32 version; 44 __be64 magic; 45 u8 id; 46 } __packed; 47 48 static u8 net_test_next_id; 49 50 #define NET_TEST_PKT_SIZE (sizeof(struct ethhdr) + sizeof(struct iphdr) + \ 51 sizeof(struct netsfhdr)) 52 #define NET_TEST_PKT_MAGIC 0xdeadcafecafedeadULL 53 #define NET_LB_TIMEOUT msecs_to_jiffies(200) 54 55 static struct sk_buff *net_test_get_skb(struct net_device *ndev, 56 struct net_packet_attrs *attr) 57 { 58 struct sk_buff *skb = NULL; 59 struct udphdr *uhdr = NULL; 60 struct tcphdr *thdr = NULL; 61 struct netsfhdr *shdr; 62 struct ethhdr *ehdr; 63 struct iphdr *ihdr; 64 int iplen, size; 65 66 size = attr->size + NET_TEST_PKT_SIZE; 67 68 if (attr->tcp) 69 size += sizeof(struct tcphdr); 70 else 71 size += sizeof(struct udphdr); 72 73 if (attr->max_size && attr->max_size > size) 74 size = attr->max_size; 75 76 skb = netdev_alloc_skb(ndev, size); 77 if (!skb) 78 return NULL; 79 80 prefetchw(skb->data); 81 82 ehdr = skb_push(skb, ETH_HLEN); 83 skb_reset_mac_header(skb); 84 85 skb_set_network_header(skb, skb->len); 86 ihdr = skb_put(skb, sizeof(*ihdr)); 87 88 skb_set_transport_header(skb, skb->len); 89 if (attr->tcp) 90 thdr = skb_put(skb, sizeof(*thdr)); 91 else 92 uhdr = skb_put(skb, sizeof(*uhdr)); 93 94 eth_zero_addr(ehdr->h_dest); 95 96 if (attr->src) 97 ether_addr_copy(ehdr->h_source, attr->src); 98 if (attr->dst) 99 ether_addr_copy(ehdr->h_dest, attr->dst); 100 101 ehdr->h_proto = htons(ETH_P_IP); 102 103 if (attr->tcp) { 104 memset(thdr, 0, sizeof(*thdr)); 105 thdr->source = htons(attr->sport); 106 thdr->dest = htons(attr->dport); 107 thdr->doff = sizeof(struct tcphdr) / 4; 108 } else { 109 uhdr->source = htons(attr->sport); 110 uhdr->dest = htons(attr->dport); 111 uhdr->len = htons(sizeof(*shdr) + sizeof(*uhdr) + attr->size); 112 if (attr->max_size) 113 uhdr->len = htons(attr->max_size - 114 (sizeof(*ihdr) + sizeof(*ehdr))); 115 uhdr->check = 0; 116 } 117 118 ihdr->ihl = 5; 119 ihdr->ttl = 32; 120 ihdr->version = 4; 121 if (attr->tcp) 122 ihdr->protocol = IPPROTO_TCP; 123 else 124 ihdr->protocol = IPPROTO_UDP; 125 iplen = sizeof(*ihdr) + sizeof(*shdr) + attr->size; 126 if (attr->tcp) 127 iplen += sizeof(*thdr); 128 else 129 iplen += sizeof(*uhdr); 130 131 if (attr->max_size) 132 iplen = attr->max_size - sizeof(*ehdr); 133 134 ihdr->tot_len = htons(iplen); 135 ihdr->frag_off = 0; 136 ihdr->saddr = htonl(attr->ip_src); 137 ihdr->daddr = htonl(attr->ip_dst); 138 ihdr->tos = 0; 139 ihdr->id = 0; 140 ip_send_check(ihdr); 141 142 shdr = skb_put(skb, sizeof(*shdr)); 143 shdr->version = 0; 144 shdr->magic = cpu_to_be64(NET_TEST_PKT_MAGIC); 145 attr->id = net_test_next_id; 146 shdr->id = net_test_next_id++; 147 148 if (attr->size) { 149 void *payload = skb_put(skb, attr->size); 150 151 memset(payload, 0, attr->size); 152 } 153 154 if (attr->max_size && attr->max_size > skb->len) { 155 size_t pad_len = attr->max_size - skb->len; 156 void *pad = skb_put(skb, pad_len); 157 158 memset(pad, 0, pad_len); 159 } 160 161 skb->csum = 0; 162 skb->ip_summed = CHECKSUM_PARTIAL; 163 if (attr->tcp) { 164 int l4len = skb->len - skb_transport_offset(skb); 165 166 thdr->check = ~tcp_v4_check(l4len, ihdr->saddr, ihdr->daddr, 0); 167 skb->csum_start = skb_transport_header(skb) - skb->head; 168 skb->csum_offset = offsetof(struct tcphdr, check); 169 170 if (attr->bad_csum) { 171 /* Force mangled checksum */ 172 if (skb_checksum_help(skb)) { 173 kfree_skb(skb); 174 return NULL; 175 } 176 177 if (thdr->check != CSUM_MANGLED_0) 178 thdr->check = CSUM_MANGLED_0; 179 else 180 thdr->check = csum16_sub(thdr->check, 181 cpu_to_be16(1)); 182 } 183 } else { 184 udp4_hwcsum(skb, ihdr->saddr, ihdr->daddr); 185 } 186 187 skb->protocol = htons(ETH_P_IP); 188 skb->pkt_type = PACKET_HOST; 189 skb->dev = ndev; 190 191 return skb; 192 } 193 194 static int net_test_loopback_validate(struct sk_buff *skb, 195 struct net_device *ndev, 196 struct packet_type *pt, 197 struct net_device *orig_ndev) 198 { 199 struct net_test_priv *tpriv = pt->af_packet_priv; 200 const unsigned char *src = tpriv->packet->src; 201 const unsigned char *dst = tpriv->packet->dst; 202 struct netsfhdr *shdr; 203 struct ethhdr *ehdr; 204 struct udphdr *uhdr; 205 struct tcphdr *thdr; 206 struct iphdr *ihdr; 207 208 skb = skb_unshare(skb, GFP_ATOMIC); 209 if (!skb) 210 goto out; 211 212 if (skb_linearize(skb)) 213 goto out; 214 if (skb_headlen(skb) < (NET_TEST_PKT_SIZE - ETH_HLEN)) 215 goto out; 216 217 ehdr = (struct ethhdr *)skb_mac_header(skb); 218 if (dst) { 219 if (!ether_addr_equal_unaligned(ehdr->h_dest, dst)) 220 goto out; 221 } 222 223 if (src) { 224 if (!ether_addr_equal_unaligned(ehdr->h_source, src)) 225 goto out; 226 } 227 228 ihdr = ip_hdr(skb); 229 if (tpriv->double_vlan) 230 ihdr = (struct iphdr *)(skb_network_header(skb) + 4); 231 232 if (tpriv->packet->tcp) { 233 if (ihdr->protocol != IPPROTO_TCP) 234 goto out; 235 236 thdr = (struct tcphdr *)((u8 *)ihdr + 4 * ihdr->ihl); 237 if (thdr->dest != htons(tpriv->packet->dport)) 238 goto out; 239 240 shdr = (struct netsfhdr *)((u8 *)thdr + sizeof(*thdr)); 241 } else { 242 if (ihdr->protocol != IPPROTO_UDP) 243 goto out; 244 245 uhdr = (struct udphdr *)((u8 *)ihdr + 4 * ihdr->ihl); 246 if (uhdr->dest != htons(tpriv->packet->dport)) 247 goto out; 248 249 shdr = (struct netsfhdr *)((u8 *)uhdr + sizeof(*uhdr)); 250 } 251 252 if (shdr->magic != cpu_to_be64(NET_TEST_PKT_MAGIC)) 253 goto out; 254 if (tpriv->packet->id != shdr->id) 255 goto out; 256 257 if (tpriv->packet->bad_csum && skb->ip_summed == CHECKSUM_UNNECESSARY) 258 tpriv->ok = -EIO; 259 else 260 tpriv->ok = true; 261 262 complete(&tpriv->comp); 263 out: 264 kfree_skb(skb); 265 return 0; 266 } 267 268 static int __net_test_loopback(struct net_device *ndev, 269 struct net_packet_attrs *attr) 270 { 271 struct net_test_priv *tpriv; 272 struct sk_buff *skb = NULL; 273 int ret = 0; 274 275 tpriv = kzalloc(sizeof(*tpriv), GFP_KERNEL); 276 if (!tpriv) 277 return -ENOMEM; 278 279 tpriv->ok = false; 280 init_completion(&tpriv->comp); 281 282 tpriv->pt.type = htons(ETH_P_IP); 283 tpriv->pt.func = net_test_loopback_validate; 284 tpriv->pt.dev = ndev; 285 tpriv->pt.af_packet_priv = tpriv; 286 tpriv->packet = attr; 287 dev_add_pack(&tpriv->pt); 288 289 skb = net_test_get_skb(ndev, attr); 290 if (!skb) { 291 ret = -ENOMEM; 292 goto cleanup; 293 } 294 295 ret = dev_direct_xmit(skb, attr->queue_mapping); 296 if (ret < 0) { 297 goto cleanup; 298 } else if (ret > 0) { 299 ret = -ENETUNREACH; 300 goto cleanup; 301 } 302 303 if (!attr->timeout) 304 attr->timeout = NET_LB_TIMEOUT; 305 306 wait_for_completion_timeout(&tpriv->comp, attr->timeout); 307 if (tpriv->ok < 0) 308 ret = tpriv->ok; 309 else if (!tpriv->ok) 310 ret = -ETIMEDOUT; 311 else 312 ret = 0; 313 314 cleanup: 315 dev_remove_pack(&tpriv->pt); 316 kfree(tpriv); 317 return ret; 318 } 319 320 static int net_test_netif_carrier(struct net_device *ndev) 321 { 322 return netif_carrier_ok(ndev) ? 0 : -ENOLINK; 323 } 324 325 static int net_test_phy_phydev(struct net_device *ndev) 326 { 327 return ndev->phydev ? 0 : -EOPNOTSUPP; 328 } 329 330 static int net_test_phy_loopback_enable(struct net_device *ndev) 331 { 332 if (!ndev->phydev) 333 return -EOPNOTSUPP; 334 335 return phy_loopback(ndev->phydev, true, 0); 336 } 337 338 static int net_test_phy_loopback_disable(struct net_device *ndev) 339 { 340 if (!ndev->phydev) 341 return -EOPNOTSUPP; 342 343 return phy_loopback(ndev->phydev, false, 0); 344 } 345 346 static int net_test_phy_loopback_udp(struct net_device *ndev) 347 { 348 struct net_packet_attrs attr = { }; 349 350 attr.dst = ndev->dev_addr; 351 return __net_test_loopback(ndev, &attr); 352 } 353 354 static int net_test_phy_loopback_udp_mtu(struct net_device *ndev) 355 { 356 struct net_packet_attrs attr = { }; 357 358 attr.dst = ndev->dev_addr; 359 attr.max_size = ndev->mtu; 360 return __net_test_loopback(ndev, &attr); 361 } 362 363 static int net_test_phy_loopback_tcp(struct net_device *ndev) 364 { 365 struct net_packet_attrs attr = { }; 366 367 attr.dst = ndev->dev_addr; 368 attr.tcp = true; 369 return __net_test_loopback(ndev, &attr); 370 } 371 372 /** 373 * net_test_phy_loopback_tcp_bad_csum - PHY loopback test with a deliberately 374 * corrupted TCP checksum 375 * @ndev: the network device to test 376 * 377 * Builds the same minimal Ethernet/IPv4/TCP frame as 378 * net_test_phy_loopback_tcp(), then flips the least-significant bit of the TCP 379 * checksum so the resulting value is provably invalid (neither 0 nor 0xFFFF). 380 * The frame is transmitted through the device’s internal PHY loopback path: 381 * 382 * test code -> MAC driver -> MAC HW -> xMII -> PHY -> 383 * internal PHY loopback -> xMII -> MAC HW -> MAC driver -> test code 384 * 385 * Result interpretation 386 * --------------------- 387 * 0 The frame is delivered to the stack and the driver reports 388 * ip_summed as CHECKSUM_NONE or CHECKSUM_COMPLETE - both are 389 * valid ways to indicate “bad checksum, let the stack verify.” 390 * -ETIMEDOUT The MAC/PHY silently dropped the frame; hardware checksum 391 * verification filtered it out before the driver saw it. 392 * -EIO The driver returned the frame with ip_summed == 393 * CHECKSUM_UNNECESSARY, falsely claiming a valid checksum and 394 * indicating a serious RX-path defect. 395 * 396 * Return: 0 on success or a negative error code on failure. 397 */ 398 static int net_test_phy_loopback_tcp_bad_csum(struct net_device *ndev) 399 { 400 struct net_packet_attrs attr = { }; 401 402 attr.dst = ndev->dev_addr; 403 attr.tcp = true; 404 attr.bad_csum = true; 405 return __net_test_loopback(ndev, &attr); 406 } 407 408 static const struct net_test { 409 char name[ETH_GSTRING_LEN]; 410 int (*fn)(struct net_device *ndev); 411 } net_selftests[] = { 412 { 413 .name = "Carrier ", 414 .fn = net_test_netif_carrier, 415 }, { 416 .name = "PHY dev is present ", 417 .fn = net_test_phy_phydev, 418 }, { 419 /* This test should be done before all PHY loopback test */ 420 .name = "PHY internal loopback, enable ", 421 .fn = net_test_phy_loopback_enable, 422 }, { 423 .name = "PHY internal loopback, UDP ", 424 .fn = net_test_phy_loopback_udp, 425 }, { 426 .name = "PHY internal loopback, MTU ", 427 .fn = net_test_phy_loopback_udp_mtu, 428 }, { 429 .name = "PHY internal loopback, TCP ", 430 .fn = net_test_phy_loopback_tcp, 431 }, { 432 .name = "PHY loopback, bad TCP csum ", 433 .fn = net_test_phy_loopback_tcp_bad_csum, 434 }, { 435 /* This test should be done after all PHY loopback test */ 436 .name = "PHY internal loopback, disable", 437 .fn = net_test_phy_loopback_disable, 438 }, 439 }; 440 441 void net_selftest(struct net_device *ndev, struct ethtool_test *etest, u64 *buf) 442 { 443 int count = net_selftest_get_count(); 444 int i; 445 446 memset(buf, 0, sizeof(*buf) * count); 447 net_test_next_id = 0; 448 449 if (etest->flags != ETH_TEST_FL_OFFLINE) { 450 netdev_err(ndev, "Only offline tests are supported\n"); 451 etest->flags |= ETH_TEST_FL_FAILED; 452 return; 453 } 454 455 456 for (i = 0; i < count; i++) { 457 buf[i] = net_selftests[i].fn(ndev); 458 if (buf[i] && (buf[i] != -EOPNOTSUPP)) 459 etest->flags |= ETH_TEST_FL_FAILED; 460 } 461 } 462 EXPORT_SYMBOL_GPL(net_selftest); 463 464 int net_selftest_get_count(void) 465 { 466 return ARRAY_SIZE(net_selftests); 467 } 468 EXPORT_SYMBOL_GPL(net_selftest_get_count); 469 470 void net_selftest_get_strings(u8 *data) 471 { 472 int i; 473 474 for (i = 0; i < net_selftest_get_count(); i++) 475 ethtool_sprintf(&data, "%2d. %s", i + 1, 476 net_selftests[i].name); 477 } 478 EXPORT_SYMBOL_GPL(net_selftest_get_strings); 479 480 MODULE_DESCRIPTION("Common library for generic PHY ethtool selftests"); 481 MODULE_LICENSE("GPL v2"); 482 MODULE_AUTHOR("Oleksij Rempel <o.rempel@pengutronix.de>"); 483