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