forked from Qortal/Brooklyn
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
412 lines
9.0 KiB
412 lines
9.0 KiB
// SPDX-License-Identifier: GPL-2.0 |
|
/* |
|
* Copyright (c) 2019 Synopsys, Inc. and/or its affiliates. |
|
* stmmac Selftests Support |
|
* |
|
* Author: Jose Abreu <[email protected]> |
|
* |
|
* Ported from stmmac by: |
|
* Copyright (C) 2021 Oleksij Rempel <[email protected]> |
|
*/ |
|
|
|
#include <linux/phy.h> |
|
#include <net/selftests.h> |
|
#include <net/tcp.h> |
|
#include <net/udp.h> |
|
|
|
struct net_packet_attrs { |
|
unsigned char *src; |
|
unsigned char *dst; |
|
u32 ip_src; |
|
u32 ip_dst; |
|
bool tcp; |
|
u16 sport; |
|
u16 dport; |
|
int timeout; |
|
int size; |
|
int max_size; |
|
u8 id; |
|
u16 queue_mapping; |
|
}; |
|
|
|
struct net_test_priv { |
|
struct net_packet_attrs *packet; |
|
struct packet_type pt; |
|
struct completion comp; |
|
int double_vlan; |
|
int vlan_id; |
|
int ok; |
|
}; |
|
|
|
struct netsfhdr { |
|
__be32 version; |
|
__be64 magic; |
|
u8 id; |
|
} __packed; |
|
|
|
static u8 net_test_next_id; |
|
|
|
#define NET_TEST_PKT_SIZE (sizeof(struct ethhdr) + sizeof(struct iphdr) + \ |
|
sizeof(struct netsfhdr)) |
|
#define NET_TEST_PKT_MAGIC 0xdeadcafecafedeadULL |
|
#define NET_LB_TIMEOUT msecs_to_jiffies(200) |
|
|
|
static struct sk_buff *net_test_get_skb(struct net_device *ndev, |
|
struct net_packet_attrs *attr) |
|
{ |
|
struct sk_buff *skb = NULL; |
|
struct udphdr *uhdr = NULL; |
|
struct tcphdr *thdr = NULL; |
|
struct netsfhdr *shdr; |
|
struct ethhdr *ehdr; |
|
struct iphdr *ihdr; |
|
int iplen, size; |
|
|
|
size = attr->size + NET_TEST_PKT_SIZE; |
|
|
|
if (attr->tcp) |
|
size += sizeof(struct tcphdr); |
|
else |
|
size += sizeof(struct udphdr); |
|
|
|
if (attr->max_size && attr->max_size > size) |
|
size = attr->max_size; |
|
|
|
skb = netdev_alloc_skb(ndev, size); |
|
if (!skb) |
|
return NULL; |
|
|
|
prefetchw(skb->data); |
|
|
|
ehdr = skb_push(skb, ETH_HLEN); |
|
skb_reset_mac_header(skb); |
|
|
|
skb_set_network_header(skb, skb->len); |
|
ihdr = skb_put(skb, sizeof(*ihdr)); |
|
|
|
skb_set_transport_header(skb, skb->len); |
|
if (attr->tcp) |
|
thdr = skb_put(skb, sizeof(*thdr)); |
|
else |
|
uhdr = skb_put(skb, sizeof(*uhdr)); |
|
|
|
eth_zero_addr(ehdr->h_dest); |
|
|
|
if (attr->src) |
|
ether_addr_copy(ehdr->h_source, attr->src); |
|
if (attr->dst) |
|
ether_addr_copy(ehdr->h_dest, attr->dst); |
|
|
|
ehdr->h_proto = htons(ETH_P_IP); |
|
|
|
if (attr->tcp) { |
|
thdr->source = htons(attr->sport); |
|
thdr->dest = htons(attr->dport); |
|
thdr->doff = sizeof(struct tcphdr) / 4; |
|
thdr->check = 0; |
|
} else { |
|
uhdr->source = htons(attr->sport); |
|
uhdr->dest = htons(attr->dport); |
|
uhdr->len = htons(sizeof(*shdr) + sizeof(*uhdr) + attr->size); |
|
if (attr->max_size) |
|
uhdr->len = htons(attr->max_size - |
|
(sizeof(*ihdr) + sizeof(*ehdr))); |
|
uhdr->check = 0; |
|
} |
|
|
|
ihdr->ihl = 5; |
|
ihdr->ttl = 32; |
|
ihdr->version = 4; |
|
if (attr->tcp) |
|
ihdr->protocol = IPPROTO_TCP; |
|
else |
|
ihdr->protocol = IPPROTO_UDP; |
|
iplen = sizeof(*ihdr) + sizeof(*shdr) + attr->size; |
|
if (attr->tcp) |
|
iplen += sizeof(*thdr); |
|
else |
|
iplen += sizeof(*uhdr); |
|
|
|
if (attr->max_size) |
|
iplen = attr->max_size - sizeof(*ehdr); |
|
|
|
ihdr->tot_len = htons(iplen); |
|
ihdr->frag_off = 0; |
|
ihdr->saddr = htonl(attr->ip_src); |
|
ihdr->daddr = htonl(attr->ip_dst); |
|
ihdr->tos = 0; |
|
ihdr->id = 0; |
|
ip_send_check(ihdr); |
|
|
|
shdr = skb_put(skb, sizeof(*shdr)); |
|
shdr->version = 0; |
|
shdr->magic = cpu_to_be64(NET_TEST_PKT_MAGIC); |
|
attr->id = net_test_next_id; |
|
shdr->id = net_test_next_id++; |
|
|
|
if (attr->size) |
|
skb_put(skb, attr->size); |
|
if (attr->max_size && attr->max_size > skb->len) |
|
skb_put(skb, attr->max_size - skb->len); |
|
|
|
skb->csum = 0; |
|
skb->ip_summed = CHECKSUM_PARTIAL; |
|
if (attr->tcp) { |
|
thdr->check = ~tcp_v4_check(skb->len, ihdr->saddr, |
|
ihdr->daddr, 0); |
|
skb->csum_start = skb_transport_header(skb) - skb->head; |
|
skb->csum_offset = offsetof(struct tcphdr, check); |
|
} else { |
|
udp4_hwcsum(skb, ihdr->saddr, ihdr->daddr); |
|
} |
|
|
|
skb->protocol = htons(ETH_P_IP); |
|
skb->pkt_type = PACKET_HOST; |
|
skb->dev = ndev; |
|
|
|
return skb; |
|
} |
|
|
|
static int net_test_loopback_validate(struct sk_buff *skb, |
|
struct net_device *ndev, |
|
struct packet_type *pt, |
|
struct net_device *orig_ndev) |
|
{ |
|
struct net_test_priv *tpriv = pt->af_packet_priv; |
|
unsigned char *src = tpriv->packet->src; |
|
unsigned char *dst = tpriv->packet->dst; |
|
struct netsfhdr *shdr; |
|
struct ethhdr *ehdr; |
|
struct udphdr *uhdr; |
|
struct tcphdr *thdr; |
|
struct iphdr *ihdr; |
|
|
|
skb = skb_unshare(skb, GFP_ATOMIC); |
|
if (!skb) |
|
goto out; |
|
|
|
if (skb_linearize(skb)) |
|
goto out; |
|
if (skb_headlen(skb) < (NET_TEST_PKT_SIZE - ETH_HLEN)) |
|
goto out; |
|
|
|
ehdr = (struct ethhdr *)skb_mac_header(skb); |
|
if (dst) { |
|
if (!ether_addr_equal_unaligned(ehdr->h_dest, dst)) |
|
goto out; |
|
} |
|
|
|
if (src) { |
|
if (!ether_addr_equal_unaligned(ehdr->h_source, src)) |
|
goto out; |
|
} |
|
|
|
ihdr = ip_hdr(skb); |
|
if (tpriv->double_vlan) |
|
ihdr = (struct iphdr *)(skb_network_header(skb) + 4); |
|
|
|
if (tpriv->packet->tcp) { |
|
if (ihdr->protocol != IPPROTO_TCP) |
|
goto out; |
|
|
|
thdr = (struct tcphdr *)((u8 *)ihdr + 4 * ihdr->ihl); |
|
if (thdr->dest != htons(tpriv->packet->dport)) |
|
goto out; |
|
|
|
shdr = (struct netsfhdr *)((u8 *)thdr + sizeof(*thdr)); |
|
} else { |
|
if (ihdr->protocol != IPPROTO_UDP) |
|
goto out; |
|
|
|
uhdr = (struct udphdr *)((u8 *)ihdr + 4 * ihdr->ihl); |
|
if (uhdr->dest != htons(tpriv->packet->dport)) |
|
goto out; |
|
|
|
shdr = (struct netsfhdr *)((u8 *)uhdr + sizeof(*uhdr)); |
|
} |
|
|
|
if (shdr->magic != cpu_to_be64(NET_TEST_PKT_MAGIC)) |
|
goto out; |
|
if (tpriv->packet->id != shdr->id) |
|
goto out; |
|
|
|
tpriv->ok = true; |
|
complete(&tpriv->comp); |
|
out: |
|
kfree_skb(skb); |
|
return 0; |
|
} |
|
|
|
static int __net_test_loopback(struct net_device *ndev, |
|
struct net_packet_attrs *attr) |
|
{ |
|
struct net_test_priv *tpriv; |
|
struct sk_buff *skb = NULL; |
|
int ret = 0; |
|
|
|
tpriv = kzalloc(sizeof(*tpriv), GFP_KERNEL); |
|
if (!tpriv) |
|
return -ENOMEM; |
|
|
|
tpriv->ok = false; |
|
init_completion(&tpriv->comp); |
|
|
|
tpriv->pt.type = htons(ETH_P_IP); |
|
tpriv->pt.func = net_test_loopback_validate; |
|
tpriv->pt.dev = ndev; |
|
tpriv->pt.af_packet_priv = tpriv; |
|
tpriv->packet = attr; |
|
dev_add_pack(&tpriv->pt); |
|
|
|
skb = net_test_get_skb(ndev, attr); |
|
if (!skb) { |
|
ret = -ENOMEM; |
|
goto cleanup; |
|
} |
|
|
|
ret = dev_direct_xmit(skb, attr->queue_mapping); |
|
if (ret < 0) { |
|
goto cleanup; |
|
} else if (ret > 0) { |
|
ret = -ENETUNREACH; |
|
goto cleanup; |
|
} |
|
|
|
if (!attr->timeout) |
|
attr->timeout = NET_LB_TIMEOUT; |
|
|
|
wait_for_completion_timeout(&tpriv->comp, attr->timeout); |
|
ret = tpriv->ok ? 0 : -ETIMEDOUT; |
|
|
|
cleanup: |
|
dev_remove_pack(&tpriv->pt); |
|
kfree(tpriv); |
|
return ret; |
|
} |
|
|
|
static int net_test_netif_carrier(struct net_device *ndev) |
|
{ |
|
return netif_carrier_ok(ndev) ? 0 : -ENOLINK; |
|
} |
|
|
|
static int net_test_phy_phydev(struct net_device *ndev) |
|
{ |
|
return ndev->phydev ? 0 : -EOPNOTSUPP; |
|
} |
|
|
|
static int net_test_phy_loopback_enable(struct net_device *ndev) |
|
{ |
|
if (!ndev->phydev) |
|
return -EOPNOTSUPP; |
|
|
|
return phy_loopback(ndev->phydev, true); |
|
} |
|
|
|
static int net_test_phy_loopback_disable(struct net_device *ndev) |
|
{ |
|
if (!ndev->phydev) |
|
return -EOPNOTSUPP; |
|
|
|
return phy_loopback(ndev->phydev, false); |
|
} |
|
|
|
static int net_test_phy_loopback_udp(struct net_device *ndev) |
|
{ |
|
struct net_packet_attrs attr = { }; |
|
|
|
attr.dst = ndev->dev_addr; |
|
return __net_test_loopback(ndev, &attr); |
|
} |
|
|
|
static int net_test_phy_loopback_udp_mtu(struct net_device *ndev) |
|
{ |
|
struct net_packet_attrs attr = { }; |
|
|
|
attr.dst = ndev->dev_addr; |
|
attr.max_size = ndev->mtu; |
|
return __net_test_loopback(ndev, &attr); |
|
} |
|
|
|
static int net_test_phy_loopback_tcp(struct net_device *ndev) |
|
{ |
|
struct net_packet_attrs attr = { }; |
|
|
|
attr.dst = ndev->dev_addr; |
|
attr.tcp = true; |
|
return __net_test_loopback(ndev, &attr); |
|
} |
|
|
|
static const struct net_test { |
|
char name[ETH_GSTRING_LEN]; |
|
int (*fn)(struct net_device *ndev); |
|
} net_selftests[] = { |
|
{ |
|
.name = "Carrier ", |
|
.fn = net_test_netif_carrier, |
|
}, { |
|
.name = "PHY dev is present ", |
|
.fn = net_test_phy_phydev, |
|
}, { |
|
/* This test should be done before all PHY loopback test */ |
|
.name = "PHY internal loopback, enable ", |
|
.fn = net_test_phy_loopback_enable, |
|
}, { |
|
.name = "PHY internal loopback, UDP ", |
|
.fn = net_test_phy_loopback_udp, |
|
}, { |
|
.name = "PHY internal loopback, MTU ", |
|
.fn = net_test_phy_loopback_udp_mtu, |
|
}, { |
|
.name = "PHY internal loopback, TCP ", |
|
.fn = net_test_phy_loopback_tcp, |
|
}, { |
|
/* This test should be done after all PHY loopback test */ |
|
.name = "PHY internal loopback, disable", |
|
.fn = net_test_phy_loopback_disable, |
|
}, |
|
}; |
|
|
|
void net_selftest(struct net_device *ndev, struct ethtool_test *etest, u64 *buf) |
|
{ |
|
int count = net_selftest_get_count(); |
|
int i; |
|
|
|
memset(buf, 0, sizeof(*buf) * count); |
|
net_test_next_id = 0; |
|
|
|
if (etest->flags != ETH_TEST_FL_OFFLINE) { |
|
netdev_err(ndev, "Only offline tests are supported\n"); |
|
etest->flags |= ETH_TEST_FL_FAILED; |
|
return; |
|
} |
|
|
|
|
|
for (i = 0; i < count; i++) { |
|
buf[i] = net_selftests[i].fn(ndev); |
|
if (buf[i] && (buf[i] != -EOPNOTSUPP)) |
|
etest->flags |= ETH_TEST_FL_FAILED; |
|
} |
|
} |
|
EXPORT_SYMBOL_GPL(net_selftest); |
|
|
|
int net_selftest_get_count(void) |
|
{ |
|
return ARRAY_SIZE(net_selftests); |
|
} |
|
EXPORT_SYMBOL_GPL(net_selftest_get_count); |
|
|
|
void net_selftest_get_strings(u8 *data) |
|
{ |
|
u8 *p = data; |
|
int i; |
|
|
|
for (i = 0; i < net_selftest_get_count(); i++) { |
|
snprintf(p, ETH_GSTRING_LEN, "%2d. %s", i + 1, |
|
net_selftests[i].name); |
|
p += ETH_GSTRING_LEN; |
|
} |
|
} |
|
EXPORT_SYMBOL_GPL(net_selftest_get_strings); |
|
|
|
MODULE_LICENSE("GPL v2"); |
|
MODULE_AUTHOR("Oleksij Rempel <[email protected]>");
|
|
|