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