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 }; 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 memset(thdr, 0, sizeof(*thdr)); 104 thdr->source = htons(attr->sport); 105 thdr->dest = htons(attr->dport); 106 thdr->doff = sizeof(struct tcphdr) / 4; 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 void *payload = skb_put(skb, attr->size); 149 150 memset(payload, 0, attr->size); 151 } 152 153 if (attr->max_size && attr->max_size > skb->len) { 154 size_t pad_len = attr->max_size - skb->len; 155 void *pad = skb_put(skb, pad_len); 156 157 memset(pad, 0, pad_len); 158 } 159 160 skb->csum = 0; 161 skb->ip_summed = CHECKSUM_PARTIAL; 162 if (attr->tcp) { 163 int l4len = skb->len - skb_transport_offset(skb); 164 165 thdr->check = ~tcp_v4_check(l4len, ihdr->saddr, ihdr->daddr, 0); 166 skb->csum_start = skb_transport_header(skb) - skb->head; 167 skb->csum_offset = offsetof(struct tcphdr, check); 168 } else { 169 udp4_hwcsum(skb, ihdr->saddr, ihdr->daddr); 170 } 171 172 skb->protocol = htons(ETH_P_IP); 173 skb->pkt_type = PACKET_HOST; 174 skb->dev = ndev; 175 176 return skb; 177 } 178 179 static int net_test_loopback_validate(struct sk_buff *skb, 180 struct net_device *ndev, 181 struct packet_type *pt, 182 struct net_device *orig_ndev) 183 { 184 struct net_test_priv *tpriv = pt->af_packet_priv; 185 const unsigned char *src = tpriv->packet->src; 186 const unsigned char *dst = tpriv->packet->dst; 187 struct netsfhdr *shdr; 188 struct ethhdr *ehdr; 189 struct udphdr *uhdr; 190 struct tcphdr *thdr; 191 struct iphdr *ihdr; 192 193 skb = skb_unshare(skb, GFP_ATOMIC); 194 if (!skb) 195 goto out; 196 197 if (skb_linearize(skb)) 198 goto out; 199 if (skb_headlen(skb) < (NET_TEST_PKT_SIZE - ETH_HLEN)) 200 goto out; 201 202 ehdr = (struct ethhdr *)skb_mac_header(skb); 203 if (dst) { 204 if (!ether_addr_equal_unaligned(ehdr->h_dest, dst)) 205 goto out; 206 } 207 208 if (src) { 209 if (!ether_addr_equal_unaligned(ehdr->h_source, src)) 210 goto out; 211 } 212 213 ihdr = ip_hdr(skb); 214 if (tpriv->double_vlan) 215 ihdr = (struct iphdr *)(skb_network_header(skb) + 4); 216 217 if (tpriv->packet->tcp) { 218 if (ihdr->protocol != IPPROTO_TCP) 219 goto out; 220 221 thdr = (struct tcphdr *)((u8 *)ihdr + 4 * ihdr->ihl); 222 if (thdr->dest != htons(tpriv->packet->dport)) 223 goto out; 224 225 shdr = (struct netsfhdr *)((u8 *)thdr + sizeof(*thdr)); 226 } else { 227 if (ihdr->protocol != IPPROTO_UDP) 228 goto out; 229 230 uhdr = (struct udphdr *)((u8 *)ihdr + 4 * ihdr->ihl); 231 if (uhdr->dest != htons(tpriv->packet->dport)) 232 goto out; 233 234 shdr = (struct netsfhdr *)((u8 *)uhdr + sizeof(*uhdr)); 235 } 236 237 if (shdr->magic != cpu_to_be64(NET_TEST_PKT_MAGIC)) 238 goto out; 239 if (tpriv->packet->id != shdr->id) 240 goto out; 241 242 tpriv->ok = true; 243 complete(&tpriv->comp); 244 out: 245 kfree_skb(skb); 246 return 0; 247 } 248 249 static int __net_test_loopback(struct net_device *ndev, 250 struct net_packet_attrs *attr) 251 { 252 struct net_test_priv *tpriv; 253 struct sk_buff *skb = NULL; 254 int ret = 0; 255 256 tpriv = kzalloc(sizeof(*tpriv), GFP_KERNEL); 257 if (!tpriv) 258 return -ENOMEM; 259 260 tpriv->ok = false; 261 init_completion(&tpriv->comp); 262 263 tpriv->pt.type = htons(ETH_P_IP); 264 tpriv->pt.func = net_test_loopback_validate; 265 tpriv->pt.dev = ndev; 266 tpriv->pt.af_packet_priv = tpriv; 267 tpriv->packet = attr; 268 dev_add_pack(&tpriv->pt); 269 270 skb = net_test_get_skb(ndev, attr); 271 if (!skb) { 272 ret = -ENOMEM; 273 goto cleanup; 274 } 275 276 ret = dev_direct_xmit(skb, attr->queue_mapping); 277 if (ret < 0) { 278 goto cleanup; 279 } else if (ret > 0) { 280 ret = -ENETUNREACH; 281 goto cleanup; 282 } 283 284 if (!attr->timeout) 285 attr->timeout = NET_LB_TIMEOUT; 286 287 wait_for_completion_timeout(&tpriv->comp, attr->timeout); 288 ret = tpriv->ok ? 0 : -ETIMEDOUT; 289 290 cleanup: 291 dev_remove_pack(&tpriv->pt); 292 kfree(tpriv); 293 return ret; 294 } 295 296 static int net_test_netif_carrier(struct net_device *ndev) 297 { 298 return netif_carrier_ok(ndev) ? 0 : -ENOLINK; 299 } 300 301 static int net_test_phy_phydev(struct net_device *ndev) 302 { 303 return ndev->phydev ? 0 : -EOPNOTSUPP; 304 } 305 306 static int net_test_phy_loopback_enable(struct net_device *ndev) 307 { 308 if (!ndev->phydev) 309 return -EOPNOTSUPP; 310 311 return phy_loopback(ndev->phydev, true, 0); 312 } 313 314 static int net_test_phy_loopback_disable(struct net_device *ndev) 315 { 316 if (!ndev->phydev) 317 return -EOPNOTSUPP; 318 319 return phy_loopback(ndev->phydev, false, 0); 320 } 321 322 static int net_test_phy_loopback_udp(struct net_device *ndev) 323 { 324 struct net_packet_attrs attr = { }; 325 326 attr.dst = ndev->dev_addr; 327 return __net_test_loopback(ndev, &attr); 328 } 329 330 static int net_test_phy_loopback_udp_mtu(struct net_device *ndev) 331 { 332 struct net_packet_attrs attr = { }; 333 334 attr.dst = ndev->dev_addr; 335 attr.max_size = ndev->mtu; 336 return __net_test_loopback(ndev, &attr); 337 } 338 339 static int net_test_phy_loopback_tcp(struct net_device *ndev) 340 { 341 struct net_packet_attrs attr = { }; 342 343 attr.dst = ndev->dev_addr; 344 attr.tcp = true; 345 return __net_test_loopback(ndev, &attr); 346 } 347 348 static const struct net_test { 349 char name[ETH_GSTRING_LEN]; 350 int (*fn)(struct net_device *ndev); 351 } net_selftests[] = { 352 { 353 .name = "Carrier ", 354 .fn = net_test_netif_carrier, 355 }, { 356 .name = "PHY dev is present ", 357 .fn = net_test_phy_phydev, 358 }, { 359 /* This test should be done before all PHY loopback test */ 360 .name = "PHY internal loopback, enable ", 361 .fn = net_test_phy_loopback_enable, 362 }, { 363 .name = "PHY internal loopback, UDP ", 364 .fn = net_test_phy_loopback_udp, 365 }, { 366 .name = "PHY internal loopback, MTU ", 367 .fn = net_test_phy_loopback_udp_mtu, 368 }, { 369 .name = "PHY internal loopback, TCP ", 370 .fn = net_test_phy_loopback_tcp, 371 }, { 372 /* This test should be done after all PHY loopback test */ 373 .name = "PHY internal loopback, disable", 374 .fn = net_test_phy_loopback_disable, 375 }, 376 }; 377 378 void net_selftest(struct net_device *ndev, struct ethtool_test *etest, u64 *buf) 379 { 380 int count = net_selftest_get_count(); 381 int i; 382 383 memset(buf, 0, sizeof(*buf) * count); 384 net_test_next_id = 0; 385 386 if (etest->flags != ETH_TEST_FL_OFFLINE) { 387 netdev_err(ndev, "Only offline tests are supported\n"); 388 etest->flags |= ETH_TEST_FL_FAILED; 389 return; 390 } 391 392 393 for (i = 0; i < count; i++) { 394 buf[i] = net_selftests[i].fn(ndev); 395 if (buf[i] && (buf[i] != -EOPNOTSUPP)) 396 etest->flags |= ETH_TEST_FL_FAILED; 397 } 398 } 399 EXPORT_SYMBOL_GPL(net_selftest); 400 401 int net_selftest_get_count(void) 402 { 403 return ARRAY_SIZE(net_selftests); 404 } 405 EXPORT_SYMBOL_GPL(net_selftest_get_count); 406 407 void net_selftest_get_strings(u8 *data) 408 { 409 int i; 410 411 for (i = 0; i < net_selftest_get_count(); i++) 412 ethtool_sprintf(&data, "%2d. %s", i + 1, 413 net_selftests[i].name); 414 } 415 EXPORT_SYMBOL_GPL(net_selftest_get_strings); 416 417 MODULE_DESCRIPTION("Common library for generic PHY ethtool selftests"); 418 MODULE_LICENSE("GPL v2"); 419 MODULE_AUTHOR("Oleksij Rempel <o.rempel@pengutronix.de>"); 420