#include <linux/bitrev.h>
#include <linux/completion.h>
#include <linux/crc32.h>
#include <linux/ethtool.h>
#include <linux/ip.h>
#include <linux/phy.h>
#include <linux/udp.h>
#include <net/pkt_cls.h>
#include <net/pkt_sched.h>
#include <net/tcp.h>
#include <net/udp.h>
#include <net/tc_act/tc_gact.h>
#include "stmmac.h"
struct stmmachdr {
__be32 version;
__be64 magic;
u8 id;
} __packed;
#define STMMAC_TEST_PKT_SIZE (sizeof(struct ethhdr) + sizeof(struct iphdr) + \
sizeof(struct stmmachdr))
#define STMMAC_TEST_PKT_MAGIC 0xdeadcafecafedeadULL
#define STMMAC_LB_TIMEOUT msecs_to_jiffies(200)
struct stmmac_packet_attrs {
int vlan;
int vlan_id_in;
int vlan_id_out;
unsigned char *src;
const unsigned char *dst;
u32 ip_src;
u32 ip_dst;
int tcp;
int sport;
int dport;
u32 exp_hash;
int dont_wait;
int timeout;
int size;
int max_size;
int remove_sa;
u8 id;
int sarc;
u16 queue_mapping;
u64 timestamp;
};
static u8 stmmac_test_next_id;
static struct sk_buff *stmmac_test_get_udp_skb(struct stmmac_priv *priv,
struct stmmac_packet_attrs *attr)
{
struct sk_buff *skb = NULL;
struct udphdr *uhdr = NULL;
struct tcphdr *thdr = NULL;
struct stmmachdr *shdr;
struct ethhdr *ehdr;
struct iphdr *ihdr;
int iplen, size;
size = attr->size + STMMAC_TEST_PKT_SIZE;
if (attr->vlan) {
size += 4;
if (attr->vlan > 1)
size += 4;
}
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(priv->dev, size);
if (!skb)
return NULL;
prefetchw(skb->data);
if (attr->vlan > 1)
ehdr = skb_push(skb, ETH_HLEN + 8);
else if (attr->vlan)
ehdr = skb_push(skb, ETH_HLEN + 4);
else if (attr->remove_sa)
ehdr = skb_push(skb, ETH_HLEN - 6);
else
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));
if (!attr->remove_sa)
eth_zero_addr(ehdr->h_source);
eth_zero_addr(ehdr->h_dest);
if (attr->src && !attr->remove_sa)
ether_addr_copy(ehdr->h_source, attr->src);
if (attr->dst)
ether_addr_copy(ehdr->h_dest, attr->dst);
if (!attr->remove_sa) {
ehdr->h_proto = htons(ETH_P_IP);
} else {
__be16 *ptr = (__be16 *)ehdr;
ptr[3] = htons(ETH_P_IP);
}
if (attr->vlan) {
__be16 *tag, *proto;
if (!attr->remove_sa) {
tag = (void *)ehdr + ETH_HLEN;
proto = (void *)ehdr + (2 * ETH_ALEN);
} else {
tag = (void *)ehdr + ETH_HLEN - 6;
proto = (void *)ehdr + ETH_ALEN;
}
proto[0] = htons(ETH_P_8021Q);
tag[0] = htons(attr->vlan_id_out);
tag[1] = htons(ETH_P_IP);
if (attr->vlan > 1) {
proto[0] = htons(ETH_P_8021AD);
tag[1] = htons(ETH_P_8021Q);
tag[2] = htons(attr->vlan_id_in);
tag[3] = 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(STMMAC_TEST_PKT_MAGIC);
attr->id = stmmac_test_next_id;
shdr->id = stmmac_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 = priv->dev;
if (attr->timestamp)
skb->tstamp = ns_to_ktime(attr->timestamp);
return skb;
}
static struct sk_buff *stmmac_test_get_arp_skb(struct stmmac_priv *priv,
struct stmmac_packet_attrs *attr)
{
__be32 ip_src = htonl(attr->ip_src);
__be32 ip_dst = htonl(attr->ip_dst);
struct sk_buff *skb = NULL;
skb = arp_create(ARPOP_REQUEST, ETH_P_ARP, ip_dst, priv->dev, ip_src,
NULL, attr->src, attr->dst);
if (!skb)
return NULL;
skb->pkt_type = PACKET_HOST;
skb->dev = priv->dev;
return skb;
}
struct stmmac_test_priv {
struct stmmac_packet_attrs *packet;
struct packet_type pt;
struct completion comp;
int double_vlan;
int vlan_id;
int ok;
};
static int stmmac_test_loopback_validate(struct sk_buff *skb,
struct net_device *ndev,
struct packet_type *pt,
struct net_device *orig_ndev)
{
struct stmmac_test_priv *tpriv = pt->af_packet_priv;
const unsigned char *dst = tpriv->packet->dst;
unsigned char *src = tpriv->packet->src;
struct stmmachdr *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) < (STMMAC_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 (tpriv->packet->sarc) {
if (!ether_addr_equal_unaligned(ehdr->h_source, ehdr->h_dest))
goto out;
} else 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 stmmachdr *)((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 stmmachdr *)((u8 *)uhdr + sizeof(*uhdr));
}
if (shdr->magic != cpu_to_be64(STMMAC_TEST_PKT_MAGIC))
goto out;
if (tpriv->packet->exp_hash && !skb->hash)
goto out;
if (tpriv->packet->id != shdr->id)
goto out;
tpriv->ok = true;
complete(&tpriv->comp);
out:
kfree_skb(skb);
return 0;
}
static int __stmmac_test_loopback(struct stmmac_priv *priv,
struct stmmac_packet_attrs *attr)
{
struct stmmac_test_priv *tpriv;
struct sk_buff *skb = NULL;
int ret = 0;
tpriv = kzalloc_obj(*tpriv);
if (!tpriv)
return -ENOMEM;
tpriv->ok = false;
init_completion(&tpriv->comp);
tpriv->pt.type = htons(ETH_P_IP);
tpriv->pt.func = stmmac_test_loopback_validate;
tpriv->pt.dev = priv->dev;
tpriv->pt.af_packet_priv = tpriv;
tpriv->packet = attr;
if (!attr->dont_wait)
dev_add_pack(&tpriv->pt);
skb = stmmac_test_get_udp_skb(priv, attr);
if (!skb) {
ret = -ENOMEM;
goto cleanup;
}
ret = dev_direct_xmit(skb, attr->queue_mapping);
if (ret)
goto cleanup;
if (attr->dont_wait)
goto cleanup;
if (!attr->timeout)
attr->timeout = STMMAC_LB_TIMEOUT;
wait_for_completion_timeout(&tpriv->comp, attr->timeout);
ret = tpriv->ok ? 0 : -ETIMEDOUT;
cleanup:
if (!attr->dont_wait)
dev_remove_pack(&tpriv->pt);
kfree(tpriv);
return ret;
}
static int stmmac_test_mac_loopback(struct stmmac_priv *priv)
{
struct stmmac_packet_attrs attr = { };
attr.dst = priv->dev->dev_addr;
return __stmmac_test_loopback(priv, &attr);
}
static int stmmac_test_phy_loopback(struct stmmac_priv *priv)
{
struct stmmac_packet_attrs attr = { };
int ret;
if (!priv->dev->phydev)
return -EOPNOTSUPP;
ret = phy_loopback(priv->dev->phydev, true, 0);
if (ret)
return ret;
attr.dst = priv->dev->dev_addr;
ret = __stmmac_test_loopback(priv, &attr);
phy_loopback(priv->dev->phydev, false, 0);
return ret;
}
static int stmmac_test_mmc(struct stmmac_priv *priv)
{
struct stmmac_counters initial, final;
int ret;
memset(&initial, 0, sizeof(initial));
memset(&final, 0, sizeof(final));
if (!priv->dma_cap.rmon)
return -EOPNOTSUPP;
stmmac_mmc_read(priv, priv->mmcaddr, &priv->mmc);
ret = stmmac_test_mac_loopback(priv);
if (ret)
return ret;
stmmac_mmc_read(priv, priv->mmcaddr, &final);
if (final.mmc_tx_framecount_g <= initial.mmc_tx_framecount_g)
return -EINVAL;
return 0;
}
static int stmmac_test_eee(struct stmmac_priv *priv)
{
struct stmmac_extra_stats *initial, *final;
int retries = 10;
int ret;
if (!priv->dma_cap.eee || !priv->eee_active)
return -EOPNOTSUPP;
initial = kzalloc_obj(*initial);
if (!initial)
return -ENOMEM;
final = kzalloc_obj(*final);
if (!final) {
ret = -ENOMEM;
goto out_free_initial;
}
memcpy(initial, &priv->xstats, sizeof(*initial));
ret = stmmac_test_mac_loopback(priv);
if (ret)
goto out_free_final;
while (--retries) {
memcpy(final, &priv->xstats, sizeof(*final));
if (final->irq_tx_path_in_lpi_mode_n >
initial->irq_tx_path_in_lpi_mode_n)
break;
msleep(100);
}
if (!retries) {
ret = -ETIMEDOUT;
goto out_free_final;
}
if (final->irq_tx_path_in_lpi_mode_n <=
initial->irq_tx_path_in_lpi_mode_n) {
ret = -EINVAL;
goto out_free_final;
}
if (final->irq_tx_path_exit_lpi_mode_n <=
initial->irq_tx_path_exit_lpi_mode_n) {
ret = -EINVAL;
goto out_free_final;
}
out_free_final:
kfree(final);
out_free_initial:
kfree(initial);
return ret;
}
static int stmmac_filter_check(struct stmmac_priv *priv)
{
if (!(priv->dev->flags & IFF_PROMISC))
return 0;
netdev_warn(priv->dev, "Test can't be run in promiscuous mode!\n");
return -EOPNOTSUPP;
}
static bool stmmac_hash_check(struct stmmac_priv *priv, unsigned char *addr)
{
int mc_offset = 32 - priv->hw->mcast_bits_log2;
struct netdev_hw_addr *ha;
u32 hash, hash_nr;
hash = bitrev32(~crc32_le(~0, addr, 6)) >> mc_offset;
hash_nr = hash >> 5;
hash = 1 << (hash & 0x1f);
netdev_for_each_mc_addr(ha, priv->dev) {
u32 nr = bitrev32(~crc32_le(~0, ha->addr, ETH_ALEN)) >> mc_offset;
if (((nr >> 5) == hash_nr) && ((1 << (nr & 0x1f)) == hash))
return false;
}
return true;
}
static bool stmmac_perfect_check(struct stmmac_priv *priv, unsigned char *addr)
{
struct netdev_hw_addr *ha;
netdev_for_each_uc_addr(ha, priv->dev) {
if (!memcmp(ha->addr, addr, ETH_ALEN))
return false;
}
return true;
}
static int stmmac_test_hfilt(struct stmmac_priv *priv)
{
unsigned char gd_addr[ETH_ALEN] = {0xf1, 0xee, 0xdd, 0xcc, 0xbb, 0xaa};
unsigned char bd_addr[ETH_ALEN] = {0xf1, 0xff, 0xff, 0xff, 0xff, 0xff};
struct stmmac_packet_attrs attr = { };
int ret, tries = 256;
ret = stmmac_filter_check(priv);
if (ret)
return ret;
if (netdev_mc_count(priv->dev) >= priv->hw->multicast_filter_bins)
return -EOPNOTSUPP;
while (--tries) {
bd_addr[ETH_ALEN - 1] = tries;
if (stmmac_hash_check(priv, bd_addr))
break;
}
if (!tries)
return -EOPNOTSUPP;
ret = dev_mc_add(priv->dev, gd_addr);
if (ret)
return ret;
attr.dst = gd_addr;
ret = __stmmac_test_loopback(priv, &attr);
if (ret)
goto cleanup;
attr.dst = bd_addr;
ret = __stmmac_test_loopback(priv, &attr);
ret = ret ? 0 : -EINVAL;
cleanup:
dev_mc_del(priv->dev, gd_addr);
return ret;
}
static int stmmac_test_pfilt(struct stmmac_priv *priv)
{
unsigned char gd_addr[ETH_ALEN] = {0xf0, 0x01, 0x44, 0x55, 0x66, 0x77};
unsigned char bd_addr[ETH_ALEN] = {0xf0, 0xff, 0xff, 0xff, 0xff, 0xff};
struct stmmac_packet_attrs attr = { };
int ret, tries = 256;
if (stmmac_filter_check(priv))
return -EOPNOTSUPP;
if (netdev_uc_count(priv->dev) >= priv->hw->unicast_filter_entries)
return -EOPNOTSUPP;
while (--tries) {
bd_addr[ETH_ALEN - 1] = tries;
if (stmmac_perfect_check(priv, bd_addr))
break;
}
if (!tries)
return -EOPNOTSUPP;
ret = dev_uc_add(priv->dev, gd_addr);
if (ret)
return ret;
attr.dst = gd_addr;
ret = __stmmac_test_loopback(priv, &attr);
if (ret)
goto cleanup;
attr.dst = bd_addr;
ret = __stmmac_test_loopback(priv, &attr);
ret = ret ? 0 : -EINVAL;
cleanup:
dev_uc_del(priv->dev, gd_addr);
return ret;
}
static int stmmac_test_mcfilt(struct stmmac_priv *priv)
{
unsigned char uc_addr[ETH_ALEN] = {0xf0, 0xff, 0xff, 0xff, 0xff, 0xff};
unsigned char mc_addr[ETH_ALEN] = {0xf1, 0xff, 0xff, 0xff, 0xff, 0xff};
struct stmmac_packet_attrs attr = { };
int ret, tries = 256;
if (stmmac_filter_check(priv))
return -EOPNOTSUPP;
if (netdev_uc_count(priv->dev) >= priv->hw->unicast_filter_entries)
return -EOPNOTSUPP;
if (netdev_mc_count(priv->dev) >= priv->hw->multicast_filter_bins)
return -EOPNOTSUPP;
while (--tries) {
mc_addr[ETH_ALEN - 1] = tries;
if (stmmac_hash_check(priv, mc_addr))
break;
}
if (!tries)
return -EOPNOTSUPP;
ret = dev_uc_add(priv->dev, uc_addr);
if (ret)
return ret;
attr.dst = uc_addr;
ret = __stmmac_test_loopback(priv, &attr);
if (ret)
goto cleanup;
attr.dst = mc_addr;
ret = __stmmac_test_loopback(priv, &attr);
ret = ret ? 0 : -EINVAL;
cleanup:
dev_uc_del(priv->dev, uc_addr);
return ret;
}
static int stmmac_test_ucfilt(struct stmmac_priv *priv)
{
unsigned char uc_addr[ETH_ALEN] = {0xf0, 0xff, 0xff, 0xff, 0xff, 0xff};
unsigned char mc_addr[ETH_ALEN] = {0xf1, 0xff, 0xff, 0xff, 0xff, 0xff};
struct stmmac_packet_attrs attr = { };
int ret, tries = 256;
if (stmmac_filter_check(priv))
return -EOPNOTSUPP;
if (netdev_uc_count(priv->dev) >= priv->hw->unicast_filter_entries)
return -EOPNOTSUPP;
if (netdev_mc_count(priv->dev) >= priv->hw->multicast_filter_bins)
return -EOPNOTSUPP;
while (--tries) {
uc_addr[ETH_ALEN - 1] = tries;
if (stmmac_perfect_check(priv, uc_addr))
break;
}
if (!tries)
return -EOPNOTSUPP;
ret = dev_mc_add(priv->dev, mc_addr);
if (ret)
return ret;
attr.dst = mc_addr;
ret = __stmmac_test_loopback(priv, &attr);
if (ret)
goto cleanup;
attr.dst = uc_addr;
ret = __stmmac_test_loopback(priv, &attr);
ret = ret ? 0 : -EINVAL;
cleanup:
dev_mc_del(priv->dev, mc_addr);
return ret;
}
static int stmmac_test_flowctrl_validate(struct sk_buff *skb,
struct net_device *ndev,
struct packet_type *pt,
struct net_device *orig_ndev)
{
struct stmmac_test_priv *tpriv = pt->af_packet_priv;
struct ethhdr *ehdr;
ehdr = (struct ethhdr *)skb_mac_header(skb);
if (!ether_addr_equal_unaligned(ehdr->h_source, orig_ndev->dev_addr))
goto out;
if (ehdr->h_proto != htons(ETH_P_PAUSE))
goto out;
tpriv->ok = true;
complete(&tpriv->comp);
out:
kfree_skb(skb);
return 0;
}
static int stmmac_test_flowctrl(struct stmmac_priv *priv)
{
unsigned char paddr[ETH_ALEN] = {0x01, 0x80, 0xC2, 0x00, 0x00, 0x01};
struct phy_device *phydev = priv->dev->phydev;
u32 rx_cnt = priv->plat->rx_queues_to_use;
struct stmmac_test_priv *tpriv;
unsigned int pkt_count;
int i, ret = 0;
if (!phydev || (!phydev->pause && !phydev->asym_pause))
return -EOPNOTSUPP;
tpriv = kzalloc_obj(*tpriv);
if (!tpriv)
return -ENOMEM;
tpriv->ok = false;
init_completion(&tpriv->comp);
tpriv->pt.type = htons(ETH_P_PAUSE);
tpriv->pt.func = stmmac_test_flowctrl_validate;
tpriv->pt.dev = priv->dev;
tpriv->pt.af_packet_priv = tpriv;
dev_add_pack(&tpriv->pt);
pkt_count = priv->plat->rx_fifo_size;
if (!pkt_count)
pkt_count = priv->dma_cap.rx_fifo_size;
pkt_count /= 1400;
pkt_count *= 2;
for (i = 0; i < rx_cnt; i++)
stmmac_stop_rx(priv, priv->ioaddr, i);
ret = dev_set_promiscuity(priv->dev, 1);
if (ret)
goto cleanup;
ret = dev_mc_add(priv->dev, paddr);
if (ret)
goto cleanup;
for (i = 0; i < pkt_count; i++) {
struct stmmac_packet_attrs attr = { };
attr.dst = priv->dev->dev_addr;
attr.dont_wait = true;
attr.size = 1400;
ret = __stmmac_test_loopback(priv, &attr);
if (ret)
goto cleanup;
if (tpriv->ok)
break;
}
msleep(200);
for (i = 0; i < rx_cnt; i++) {
struct stmmac_channel *ch = &priv->channel[i];
u32 tail;
tail = priv->dma_conf.rx_queue[i].dma_rx_phy +
(priv->dma_conf.dma_rx_size * sizeof(struct dma_desc));
stmmac_set_rx_tail_ptr(priv, priv->ioaddr, tail, i);
stmmac_start_rx(priv, priv->ioaddr, i);
local_bh_disable();
napi_schedule(&ch->rx_napi);
local_bh_enable();
}
wait_for_completion_timeout(&tpriv->comp, STMMAC_LB_TIMEOUT);
ret = tpriv->ok ? 0 : -ETIMEDOUT;
cleanup:
dev_mc_del(priv->dev, paddr);
dev_set_promiscuity(priv->dev, -1);
dev_remove_pack(&tpriv->pt);
kfree(tpriv);
return ret;
}
static int stmmac_test_rss(struct stmmac_priv *priv)
{
struct stmmac_packet_attrs attr = { };
if (!priv->dma_cap.rssen || !priv->rss.enable)
return -EOPNOTSUPP;
attr.dst = priv->dev->dev_addr;
attr.exp_hash = true;
attr.sport = 0x321;
attr.dport = 0x123;
return __stmmac_test_loopback(priv, &attr);
}
static int stmmac_test_vlan_validate(struct sk_buff *skb,
struct net_device *ndev,
struct packet_type *pt,
struct net_device *orig_ndev)
{
struct stmmac_test_priv *tpriv = pt->af_packet_priv;
struct stmmachdr *shdr;
struct ethhdr *ehdr;
struct udphdr *uhdr;
struct iphdr *ihdr;
u16 proto;
proto = tpriv->double_vlan ? ETH_P_8021AD : ETH_P_8021Q;
skb = skb_unshare(skb, GFP_ATOMIC);
if (!skb)
goto out;
if (skb_linearize(skb))
goto out;
if (skb_headlen(skb) < (STMMAC_TEST_PKT_SIZE - ETH_HLEN))
goto out;
if (tpriv->vlan_id) {
if (skb->vlan_proto != htons(proto))
goto out;
if (skb->vlan_tci != tpriv->vlan_id) {
tpriv->ok = false;
complete(&tpriv->comp);
goto out;
}
}
ehdr = (struct ethhdr *)skb_mac_header(skb);
if (!ether_addr_equal_unaligned(ehdr->h_dest, tpriv->packet->dst))
goto out;
ihdr = ip_hdr(skb);
if (tpriv->double_vlan)
ihdr = (struct iphdr *)(skb_network_header(skb) + 4);
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 stmmachdr *)((u8 *)uhdr + sizeof(*uhdr));
if (shdr->magic != cpu_to_be64(STMMAC_TEST_PKT_MAGIC))
goto out;
tpriv->ok = true;
complete(&tpriv->comp);
out:
kfree_skb(skb);
return 0;
}
static int __stmmac_test_vlanfilt(struct stmmac_priv *priv)
{
struct stmmac_packet_attrs attr = { };
struct stmmac_test_priv *tpriv;
struct sk_buff *skb = NULL;
int ret = 0, i;
tpriv = kzalloc_obj(*tpriv);
if (!tpriv)
return -ENOMEM;
tpriv->ok = false;
init_completion(&tpriv->comp);
tpriv->pt.type = htons(ETH_P_IP);
tpriv->pt.func = stmmac_test_vlan_validate;
tpriv->pt.dev = priv->dev;
tpriv->pt.af_packet_priv = tpriv;
tpriv->packet = &attr;
tpriv->vlan_id = 0x123;
dev_add_pack(&tpriv->pt);
ret = vlan_vid_add(priv->dev, htons(ETH_P_8021Q), tpriv->vlan_id);
if (ret)
goto cleanup;
for (i = 0; i < 4; i++) {
attr.vlan = 1;
attr.vlan_id_out = tpriv->vlan_id + i;
attr.dst = priv->dev->dev_addr;
attr.sport = 9;
attr.dport = 9;
skb = stmmac_test_get_udp_skb(priv, &attr);
if (!skb) {
ret = -ENOMEM;
goto vlan_del;
}
ret = dev_direct_xmit(skb, 0);
if (ret)
goto vlan_del;
wait_for_completion_timeout(&tpriv->comp, STMMAC_LB_TIMEOUT);
ret = tpriv->ok ? 0 : -ETIMEDOUT;
if (ret && !i) {
goto vlan_del;
} else if (!ret && i) {
ret = -EINVAL;
goto vlan_del;
} else {
ret = 0;
}
tpriv->ok = false;
}
vlan_del:
vlan_vid_del(priv->dev, htons(ETH_P_8021Q), tpriv->vlan_id);
cleanup:
dev_remove_pack(&tpriv->pt);
kfree(tpriv);
return ret;
}
static int stmmac_test_vlanfilt(struct stmmac_priv *priv)
{
if (!priv->dma_cap.vlhash)
return -EOPNOTSUPP;
return __stmmac_test_vlanfilt(priv);
}
static int stmmac_test_vlanfilt_perfect(struct stmmac_priv *priv)
{
int ret, prev_cap = priv->dma_cap.vlhash;
if (!(priv->dev->features & NETIF_F_HW_VLAN_CTAG_FILTER))
return -EOPNOTSUPP;
priv->dma_cap.vlhash = 0;
ret = __stmmac_test_vlanfilt(priv);
priv->dma_cap.vlhash = prev_cap;
return ret;
}
static int __stmmac_test_dvlanfilt(struct stmmac_priv *priv)
{
struct stmmac_packet_attrs attr = { };
struct stmmac_test_priv *tpriv;
struct sk_buff *skb = NULL;
int ret = 0, i;
tpriv = kzalloc_obj(*tpriv);
if (!tpriv)
return -ENOMEM;
tpriv->ok = false;
tpriv->double_vlan = true;
init_completion(&tpriv->comp);
tpriv->pt.type = htons(ETH_P_8021Q);
tpriv->pt.func = stmmac_test_vlan_validate;
tpriv->pt.dev = priv->dev;
tpriv->pt.af_packet_priv = tpriv;
tpriv->packet = &attr;
tpriv->vlan_id = 0x123;
dev_add_pack(&tpriv->pt);
ret = vlan_vid_add(priv->dev, htons(ETH_P_8021AD), tpriv->vlan_id);
if (ret)
goto cleanup;
for (i = 0; i < 4; i++) {
attr.vlan = 2;
attr.vlan_id_out = tpriv->vlan_id + i;
attr.dst = priv->dev->dev_addr;
attr.sport = 9;
attr.dport = 9;
skb = stmmac_test_get_udp_skb(priv, &attr);
if (!skb) {
ret = -ENOMEM;
goto vlan_del;
}
ret = dev_direct_xmit(skb, 0);
if (ret)
goto vlan_del;
wait_for_completion_timeout(&tpriv->comp, STMMAC_LB_TIMEOUT);
ret = tpriv->ok ? 0 : -ETIMEDOUT;
if (ret && !i) {
goto vlan_del;
} else if (!ret && i) {
ret = -EINVAL;
goto vlan_del;
} else {
ret = 0;
}
tpriv->ok = false;
}
vlan_del:
vlan_vid_del(priv->dev, htons(ETH_P_8021AD), tpriv->vlan_id);
cleanup:
dev_remove_pack(&tpriv->pt);
kfree(tpriv);
return ret;
}
static int stmmac_test_dvlanfilt(struct stmmac_priv *priv)
{
if (!priv->dma_cap.vlhash)
return -EOPNOTSUPP;
return __stmmac_test_dvlanfilt(priv);
}
static int stmmac_test_dvlanfilt_perfect(struct stmmac_priv *priv)
{
int ret, prev_cap = priv->dma_cap.vlhash;
if (!(priv->dev->features & NETIF_F_HW_VLAN_STAG_FILTER))
return -EOPNOTSUPP;
priv->dma_cap.vlhash = 0;
ret = __stmmac_test_dvlanfilt(priv);
priv->dma_cap.vlhash = prev_cap;
return ret;
}
#ifdef CONFIG_NET_CLS_ACT
static int stmmac_test_rxp(struct stmmac_priv *priv)
{
unsigned char addr[ETH_ALEN] = {0xde, 0xad, 0xbe, 0xef, 0x00, 0x00};
struct tc_cls_u32_offload cls_u32 = { };
struct stmmac_packet_attrs attr = { };
struct tc_action **actions;
struct tc_u32_sel *sel;
struct tcf_gact *gact;
struct tcf_exts *exts;
int ret, i, nk = 1;
if (!tc_can_offload(priv->dev))
return -EOPNOTSUPP;
if (!priv->dma_cap.frpsel)
return -EOPNOTSUPP;
sel = kzalloc_flex(*sel, keys, nk);
if (!sel)
return -ENOMEM;
exts = kzalloc_obj(*exts);
if (!exts) {
ret = -ENOMEM;
goto cleanup_sel;
}
actions = kzalloc_objs(*actions, nk);
if (!actions) {
ret = -ENOMEM;
goto cleanup_exts;
}
gact = kzalloc_objs(*gact, nk);
if (!gact) {
ret = -ENOMEM;
goto cleanup_actions;
}
cls_u32.command = TC_CLSU32_NEW_KNODE;
cls_u32.common.chain_index = 0;
cls_u32.common.protocol = htons(ETH_P_ALL);
cls_u32.knode.exts = exts;
cls_u32.knode.sel = sel;
cls_u32.knode.handle = 0x123;
exts->nr_actions = nk;
exts->actions = actions;
for (i = 0; i < nk; i++) {
actions[i] = (struct tc_action *)&gact[i];
gact->tcf_action = TC_ACT_SHOT;
}
sel->nkeys = nk;
sel->offshift = 0;
sel->keys[0].off = 6;
sel->keys[0].val = htonl(0xdeadbeef);
sel->keys[0].mask = ~0x0;
ret = stmmac_tc_setup_cls_u32(priv, priv, &cls_u32);
if (ret)
goto cleanup_act;
attr.dst = priv->dev->dev_addr;
attr.src = addr;
ret = __stmmac_test_loopback(priv, &attr);
ret = ret ? 0 : -EINVAL;
cls_u32.command = TC_CLSU32_DELETE_KNODE;
stmmac_tc_setup_cls_u32(priv, priv, &cls_u32);
cleanup_act:
kfree(gact);
cleanup_actions:
kfree(actions);
cleanup_exts:
kfree(exts);
cleanup_sel:
kfree(sel);
return ret;
}
#else
static int stmmac_test_rxp(struct stmmac_priv *priv)
{
return -EOPNOTSUPP;
}
#endif
static int stmmac_test_desc_sai(struct stmmac_priv *priv)
{
unsigned char src[ETH_ALEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
struct stmmac_packet_attrs attr = { };
int ret;
if (!priv->dma_cap.vlins)
return -EOPNOTSUPP;
attr.remove_sa = true;
attr.sarc = true;
attr.src = src;
attr.dst = priv->dev->dev_addr;
priv->sarc_type = 0x1;
ret = __stmmac_test_loopback(priv, &attr);
priv->sarc_type = 0x0;
return ret;
}
static int stmmac_test_desc_sar(struct stmmac_priv *priv)
{
unsigned char src[ETH_ALEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
struct stmmac_packet_attrs attr = { };
int ret;
if (!priv->dma_cap.vlins)
return -EOPNOTSUPP;
attr.sarc = true;
attr.src = src;
attr.dst = priv->dev->dev_addr;
priv->sarc_type = 0x2;
ret = __stmmac_test_loopback(priv, &attr);
priv->sarc_type = 0x0;
return ret;
}
static int stmmac_test_reg_sai(struct stmmac_priv *priv)
{
unsigned char src[ETH_ALEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
struct stmmac_packet_attrs attr = { };
int ret;
if (!priv->dma_cap.vlins)
return -EOPNOTSUPP;
attr.remove_sa = true;
attr.sarc = true;
attr.src = src;
attr.dst = priv->dev->dev_addr;
if (stmmac_sarc_configure(priv, priv->ioaddr, 0x2))
return -EOPNOTSUPP;
ret = __stmmac_test_loopback(priv, &attr);
stmmac_sarc_configure(priv, priv->ioaddr, 0x0);
return ret;
}
static int stmmac_test_reg_sar(struct stmmac_priv *priv)
{
unsigned char src[ETH_ALEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
struct stmmac_packet_attrs attr = { };
int ret;
if (!priv->dma_cap.vlins)
return -EOPNOTSUPP;
attr.sarc = true;
attr.src = src;
attr.dst = priv->dev->dev_addr;
if (stmmac_sarc_configure(priv, priv->ioaddr, 0x3))
return -EOPNOTSUPP;
ret = __stmmac_test_loopback(priv, &attr);
stmmac_sarc_configure(priv, priv->ioaddr, 0x0);
return ret;
}
static int stmmac_test_vlanoff_common(struct stmmac_priv *priv, bool svlan)
{
struct stmmac_packet_attrs attr = { };
struct stmmac_test_priv *tpriv;
struct sk_buff *skb = NULL;
int ret = 0;
u16 proto;
if (!priv->dma_cap.vlins)
return -EOPNOTSUPP;
tpriv = kzalloc_obj(*tpriv);
if (!tpriv)
return -ENOMEM;
proto = svlan ? ETH_P_8021AD : ETH_P_8021Q;
tpriv->ok = false;
tpriv->double_vlan = svlan;
init_completion(&tpriv->comp);
tpriv->pt.type = svlan ? htons(ETH_P_8021Q) : htons(ETH_P_IP);
tpriv->pt.func = stmmac_test_vlan_validate;
tpriv->pt.dev = priv->dev;
tpriv->pt.af_packet_priv = tpriv;
tpriv->packet = &attr;
tpriv->vlan_id = 0x123;
dev_add_pack(&tpriv->pt);
ret = vlan_vid_add(priv->dev, htons(proto), tpriv->vlan_id);
if (ret)
goto cleanup;
attr.dst = priv->dev->dev_addr;
skb = stmmac_test_get_udp_skb(priv, &attr);
if (!skb) {
ret = -ENOMEM;
goto vlan_del;
}
__vlan_hwaccel_put_tag(skb, htons(proto), tpriv->vlan_id);
skb->protocol = htons(proto);
ret = dev_direct_xmit(skb, 0);
if (ret)
goto vlan_del;
wait_for_completion_timeout(&tpriv->comp, STMMAC_LB_TIMEOUT);
ret = tpriv->ok ? 0 : -ETIMEDOUT;
vlan_del:
vlan_vid_del(priv->dev, htons(proto), tpriv->vlan_id);
cleanup:
dev_remove_pack(&tpriv->pt);
kfree(tpriv);
return ret;
}
static int stmmac_test_vlanoff(struct stmmac_priv *priv)
{
return stmmac_test_vlanoff_common(priv, false);
}
static int stmmac_test_svlanoff(struct stmmac_priv *priv)
{
if (!priv->dma_cap.dvlan)
return -EOPNOTSUPP;
return stmmac_test_vlanoff_common(priv, true);
}
#ifdef CONFIG_NET_CLS_ACT
static int __stmmac_test_l3filt(struct stmmac_priv *priv, u32 dst, u32 src,
u32 dst_mask, u32 src_mask)
{
struct flow_dissector_key_ipv4_addrs key, mask;
unsigned long dummy_cookie = 0xdeadbeef;
struct stmmac_packet_attrs attr = { };
struct flow_dissector *dissector;
struct flow_cls_offload *cls;
int ret, old_enable = 0;
struct flow_rule *rule;
if (!tc_can_offload(priv->dev))
return -EOPNOTSUPP;
if (!priv->dma_cap.l3l4fnum)
return -EOPNOTSUPP;
if (priv->rss.enable) {
old_enable = priv->rss.enable;
priv->rss.enable = false;
stmmac_rss_configure(priv, priv->hw, NULL,
priv->plat->rx_queues_to_use);
}
dissector = kzalloc_obj(*dissector);
if (!dissector) {
ret = -ENOMEM;
goto cleanup_rss;
}
dissector->used_keys |= (1ULL << FLOW_DISSECTOR_KEY_IPV4_ADDRS);
dissector->offset[FLOW_DISSECTOR_KEY_IPV4_ADDRS] = 0;
cls = kzalloc_obj(*cls);
if (!cls) {
ret = -ENOMEM;
goto cleanup_dissector;
}
cls->common.chain_index = 0;
cls->command = FLOW_CLS_REPLACE;
cls->cookie = dummy_cookie;
rule = kzalloc_flex(*rule, action.entries, 1);
if (!rule) {
ret = -ENOMEM;
goto cleanup_cls;
}
rule->match.dissector = dissector;
rule->match.key = (void *)&key;
rule->match.mask = (void *)&mask;
key.src = htonl(src);
key.dst = htonl(dst);
mask.src = src_mask;
mask.dst = dst_mask;
cls->rule = rule;
rule->action.entries[0].id = FLOW_ACTION_DROP;
rule->action.entries[0].hw_stats = FLOW_ACTION_HW_STATS_ANY;
rule->action.num_entries = 1;
attr.dst = priv->dev->dev_addr;
attr.ip_dst = dst;
attr.ip_src = src;
ret = __stmmac_test_loopback(priv, &attr);
if (ret)
goto cleanup_rule;
ret = stmmac_tc_setup_cls(priv, priv, cls);
if (ret)
goto cleanup_rule;
ret = __stmmac_test_loopback(priv, &attr);
ret = ret ? 0 : -EINVAL;
cls->command = FLOW_CLS_DESTROY;
stmmac_tc_setup_cls(priv, priv, cls);
cleanup_rule:
kfree(rule);
cleanup_cls:
kfree(cls);
cleanup_dissector:
kfree(dissector);
cleanup_rss:
if (old_enable) {
priv->rss.enable = old_enable;
stmmac_rss_configure(priv, priv->hw, &priv->rss,
priv->plat->rx_queues_to_use);
}
return ret;
}
#else
static int __stmmac_test_l3filt(struct stmmac_priv *priv, u32 dst, u32 src,
u32 dst_mask, u32 src_mask)
{
return -EOPNOTSUPP;
}
#endif
static int stmmac_test_l3filt_da(struct stmmac_priv *priv)
{
u32 addr = 0x10203040;
return __stmmac_test_l3filt(priv, addr, 0, ~0, 0);
}
static int stmmac_test_l3filt_sa(struct stmmac_priv *priv)
{
u32 addr = 0x10203040;
return __stmmac_test_l3filt(priv, 0, addr, 0, ~0);
}
#ifdef CONFIG_NET_CLS_ACT
static int __stmmac_test_l4filt(struct stmmac_priv *priv, u32 dst, u32 src,
u32 dst_mask, u32 src_mask, bool udp)
{
struct {
struct flow_dissector_key_basic bkey;
struct flow_dissector_key_ports key;
} __aligned(BITS_PER_LONG / 8) keys;
struct {
struct flow_dissector_key_basic bmask;
struct flow_dissector_key_ports mask;
} __aligned(BITS_PER_LONG / 8) masks;
unsigned long dummy_cookie = 0xdeadbeef;
struct stmmac_packet_attrs attr = { };
struct flow_dissector *dissector;
struct flow_cls_offload *cls;
int ret, old_enable = 0;
struct flow_rule *rule;
if (!tc_can_offload(priv->dev))
return -EOPNOTSUPP;
if (!priv->dma_cap.l3l4fnum)
return -EOPNOTSUPP;
if (priv->rss.enable) {
old_enable = priv->rss.enable;
priv->rss.enable = false;
stmmac_rss_configure(priv, priv->hw, NULL,
priv->plat->rx_queues_to_use);
}
dissector = kzalloc_obj(*dissector);
if (!dissector) {
ret = -ENOMEM;
goto cleanup_rss;
}
dissector->used_keys |= (1ULL << FLOW_DISSECTOR_KEY_BASIC);
dissector->used_keys |= (1ULL << FLOW_DISSECTOR_KEY_PORTS);
dissector->offset[FLOW_DISSECTOR_KEY_BASIC] = 0;
dissector->offset[FLOW_DISSECTOR_KEY_PORTS] = offsetof(typeof(keys), key);
cls = kzalloc_obj(*cls);
if (!cls) {
ret = -ENOMEM;
goto cleanup_dissector;
}
cls->common.chain_index = 0;
cls->command = FLOW_CLS_REPLACE;
cls->cookie = dummy_cookie;
rule = kzalloc_flex(*rule, action.entries, 1);
if (!rule) {
ret = -ENOMEM;
goto cleanup_cls;
}
rule->match.dissector = dissector;
rule->match.key = (void *)&keys;
rule->match.mask = (void *)&masks;
keys.bkey.ip_proto = udp ? IPPROTO_UDP : IPPROTO_TCP;
keys.key.src = htons(src);
keys.key.dst = htons(dst);
masks.mask.src = src_mask;
masks.mask.dst = dst_mask;
cls->rule = rule;
rule->action.entries[0].id = FLOW_ACTION_DROP;
rule->action.entries[0].hw_stats = FLOW_ACTION_HW_STATS_ANY;
rule->action.num_entries = 1;
attr.dst = priv->dev->dev_addr;
attr.tcp = !udp;
attr.sport = src;
attr.dport = dst;
attr.ip_dst = 0;
ret = __stmmac_test_loopback(priv, &attr);
if (ret)
goto cleanup_rule;
ret = stmmac_tc_setup_cls(priv, priv, cls);
if (ret)
goto cleanup_rule;
ret = __stmmac_test_loopback(priv, &attr);
ret = ret ? 0 : -EINVAL;
cls->command = FLOW_CLS_DESTROY;
stmmac_tc_setup_cls(priv, priv, cls);
cleanup_rule:
kfree(rule);
cleanup_cls:
kfree(cls);
cleanup_dissector:
kfree(dissector);
cleanup_rss:
if (old_enable) {
priv->rss.enable = old_enable;
stmmac_rss_configure(priv, priv->hw, &priv->rss,
priv->plat->rx_queues_to_use);
}
return ret;
}
#else
static int __stmmac_test_l4filt(struct stmmac_priv *priv, u32 dst, u32 src,
u32 dst_mask, u32 src_mask, bool udp)
{
return -EOPNOTSUPP;
}
#endif
static int stmmac_test_l4filt_da_tcp(struct stmmac_priv *priv)
{
u16 dummy_port = 0x123;
return __stmmac_test_l4filt(priv, dummy_port, 0, ~0, 0, false);
}
static int stmmac_test_l4filt_sa_tcp(struct stmmac_priv *priv)
{
u16 dummy_port = 0x123;
return __stmmac_test_l4filt(priv, 0, dummy_port, 0, ~0, false);
}
static int stmmac_test_l4filt_da_udp(struct stmmac_priv *priv)
{
u16 dummy_port = 0x123;
return __stmmac_test_l4filt(priv, dummy_port, 0, ~0, 0, true);
}
static int stmmac_test_l4filt_sa_udp(struct stmmac_priv *priv)
{
u16 dummy_port = 0x123;
return __stmmac_test_l4filt(priv, 0, dummy_port, 0, ~0, true);
}
static int stmmac_test_arp_validate(struct sk_buff *skb,
struct net_device *ndev,
struct packet_type *pt,
struct net_device *orig_ndev)
{
struct stmmac_test_priv *tpriv = pt->af_packet_priv;
struct ethhdr *ehdr;
struct arphdr *ahdr;
ehdr = (struct ethhdr *)skb_mac_header(skb);
if (!ether_addr_equal_unaligned(ehdr->h_dest, tpriv->packet->src))
goto out;
ahdr = arp_hdr(skb);
if (ahdr->ar_op != htons(ARPOP_REPLY))
goto out;
tpriv->ok = true;
complete(&tpriv->comp);
out:
kfree_skb(skb);
return 0;
}
static int stmmac_test_arpoffload(struct stmmac_priv *priv)
{
unsigned char src[ETH_ALEN] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06};
unsigned char dst[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
struct stmmac_packet_attrs attr = { };
struct stmmac_test_priv *tpriv;
struct sk_buff *skb = NULL;
u32 ip_addr = 0xdeadcafe;
u32 ip_src = 0xdeadbeef;
int ret;
if (!priv->dma_cap.arpoffsel)
return -EOPNOTSUPP;
tpriv = kzalloc_obj(*tpriv);
if (!tpriv)
return -ENOMEM;
tpriv->ok = false;
init_completion(&tpriv->comp);
tpriv->pt.type = htons(ETH_P_ARP);
tpriv->pt.func = stmmac_test_arp_validate;
tpriv->pt.dev = priv->dev;
tpriv->pt.af_packet_priv = tpriv;
tpriv->packet = &attr;
dev_add_pack(&tpriv->pt);
attr.src = src;
attr.ip_src = ip_src;
attr.dst = dst;
attr.ip_dst = ip_addr;
skb = stmmac_test_get_arp_skb(priv, &attr);
if (!skb) {
ret = -ENOMEM;
goto cleanup;
}
ret = stmmac_set_arp_offload(priv, priv->hw, true, ip_addr);
if (ret) {
kfree_skb(skb);
goto cleanup;
}
ret = dev_set_promiscuity(priv->dev, 1);
if (ret) {
kfree_skb(skb);
goto cleanup;
}
ret = dev_direct_xmit(skb, 0);
if (ret)
goto cleanup_promisc;
wait_for_completion_timeout(&tpriv->comp, STMMAC_LB_TIMEOUT);
ret = tpriv->ok ? 0 : -ETIMEDOUT;
cleanup_promisc:
dev_set_promiscuity(priv->dev, -1);
cleanup:
stmmac_set_arp_offload(priv, priv->hw, false, 0x0);
dev_remove_pack(&tpriv->pt);
kfree(tpriv);
return ret;
}
static int __stmmac_test_jumbo(struct stmmac_priv *priv, u16 queue)
{
struct stmmac_packet_attrs attr = { };
int size = priv->dma_conf.dma_buf_sz;
attr.dst = priv->dev->dev_addr;
attr.max_size = size - ETH_FCS_LEN;
attr.queue_mapping = queue;
return __stmmac_test_loopback(priv, &attr);
}
static int stmmac_test_jumbo(struct stmmac_priv *priv)
{
return __stmmac_test_jumbo(priv, 0);
}
static int stmmac_test_mjumbo(struct stmmac_priv *priv)
{
u32 chan, tx_cnt = priv->plat->tx_queues_to_use;
int ret;
if (tx_cnt <= 1)
return -EOPNOTSUPP;
for (chan = 0; chan < tx_cnt; chan++) {
ret = __stmmac_test_jumbo(priv, chan);
if (ret)
return ret;
}
return 0;
}
static int stmmac_test_sph(struct stmmac_priv *priv)
{
unsigned long cnt_end, cnt_start = priv->xstats.rx_split_hdr_pkt_n;
struct stmmac_packet_attrs attr = { };
int ret;
if (!priv->sph_active)
return -EOPNOTSUPP;
attr.dst = priv->dev->dev_addr;
attr.tcp = false;
ret = __stmmac_test_loopback(priv, &attr);
if (ret)
return ret;
cnt_end = priv->xstats.rx_split_hdr_pkt_n;
if (cnt_end <= cnt_start)
return -EINVAL;
cnt_start = cnt_end;
attr.dst = priv->dev->dev_addr;
attr.tcp = true;
ret = __stmmac_test_loopback(priv, &attr);
if (ret)
return ret;
cnt_end = priv->xstats.rx_split_hdr_pkt_n;
if (cnt_end <= cnt_start)
return -EINVAL;
return 0;
}
static int stmmac_test_tbs(struct stmmac_priv *priv)
{
#define STMMAC_TBS_LT_OFFSET (500 * 1000 * 1000)
struct stmmac_packet_attrs attr = { };
struct tc_etf_qopt_offload qopt;
u64 start_time, curr_time = 0;
unsigned long flags;
int ret, i;
if (!priv->hwts_tx_en)
return -EOPNOTSUPP;
for (i = 0; i < priv->plat->tx_queues_to_use; i++)
if (priv->dma_conf.tx_queue[i].tbs & STMMAC_TBS_AVAIL)
break;
if (i >= priv->plat->tx_queues_to_use)
return -EOPNOTSUPP;
qopt.enable = true;
qopt.queue = i;
ret = stmmac_tc_setup_etf(priv, priv, &qopt);
if (ret)
return ret;
read_lock_irqsave(&priv->ptp_lock, flags);
stmmac_get_systime(priv, priv->ptpaddr, &curr_time);
read_unlock_irqrestore(&priv->ptp_lock, flags);
if (!curr_time) {
ret = -EOPNOTSUPP;
goto fail_disable;
}
start_time = curr_time;
curr_time += STMMAC_TBS_LT_OFFSET;
attr.dst = priv->dev->dev_addr;
attr.timestamp = curr_time;
attr.timeout = nsecs_to_jiffies(2 * STMMAC_TBS_LT_OFFSET);
attr.queue_mapping = i;
ret = __stmmac_test_loopback(priv, &attr);
if (ret)
goto fail_disable;
read_lock_irqsave(&priv->ptp_lock, flags);
stmmac_get_systime(priv, priv->ptpaddr, &curr_time);
read_unlock_irqrestore(&priv->ptp_lock, flags);
if ((curr_time - start_time) < STMMAC_TBS_LT_OFFSET)
ret = -EINVAL;
fail_disable:
qopt.enable = false;
stmmac_tc_setup_etf(priv, priv, &qopt);
return ret;
}
#define STMMAC_LOOPBACK_NONE 0
#define STMMAC_LOOPBACK_MAC 1
#define STMMAC_LOOPBACK_PHY 2
static const struct stmmac_test {
char name[ETH_GSTRING_LEN];
int lb;
int (*fn)(struct stmmac_priv *priv);
} stmmac_selftests[] = {
{
.name = "MAC Loopback ",
.lb = STMMAC_LOOPBACK_MAC,
.fn = stmmac_test_mac_loopback,
}, {
.name = "PHY Loopback ",
.lb = STMMAC_LOOPBACK_NONE,
.fn = stmmac_test_phy_loopback,
}, {
.name = "MMC Counters ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_mmc,
}, {
.name = "EEE ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_eee,
}, {
.name = "Hash Filter MC ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_hfilt,
}, {
.name = "Perfect Filter UC ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_pfilt,
}, {
.name = "MC Filter ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_mcfilt,
}, {
.name = "UC Filter ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_ucfilt,
}, {
.name = "Flow Control ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_flowctrl,
}, {
.name = "RSS ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_rss,
}, {
.name = "VLAN Filtering ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_vlanfilt,
}, {
.name = "VLAN Filtering (perf) ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_vlanfilt_perfect,
}, {
.name = "Double VLAN Filter ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_dvlanfilt,
}, {
.name = "Double VLAN Filter (perf) ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_dvlanfilt_perfect,
}, {
.name = "Flexible RX Parser ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_rxp,
}, {
.name = "SA Insertion (desc) ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_desc_sai,
}, {
.name = "SA Replacement (desc) ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_desc_sar,
}, {
.name = "SA Insertion (reg) ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_reg_sai,
}, {
.name = "SA Replacement (reg) ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_reg_sar,
}, {
.name = "VLAN TX Insertion ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_vlanoff,
}, {
.name = "SVLAN TX Insertion ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_svlanoff,
}, {
.name = "L3 DA Filtering ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_l3filt_da,
}, {
.name = "L3 SA Filtering ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_l3filt_sa,
}, {
.name = "L4 DA TCP Filtering ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_l4filt_da_tcp,
}, {
.name = "L4 SA TCP Filtering ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_l4filt_sa_tcp,
}, {
.name = "L4 DA UDP Filtering ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_l4filt_da_udp,
}, {
.name = "L4 SA UDP Filtering ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_l4filt_sa_udp,
}, {
.name = "ARP Offload ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_arpoffload,
}, {
.name = "Jumbo Frame ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_jumbo,
}, {
.name = "Multichannel Jumbo ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_mjumbo,
}, {
.name = "Split Header ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_sph,
}, {
.name = "TBS (ETF Scheduler) ",
.lb = STMMAC_LOOPBACK_PHY,
.fn = stmmac_test_tbs,
},
};
void stmmac_selftest_run(struct net_device *dev,
struct ethtool_test *etest, u64 *buf)
{
struct stmmac_priv *priv = netdev_priv(dev);
int count = stmmac_selftest_get_count(priv);
int i, ret;
memset(buf, 0, sizeof(*buf) * count);
stmmac_test_next_id = 0;
if (etest->flags != ETH_TEST_FL_OFFLINE) {
netdev_err(priv->dev, "Only offline tests are supported\n");
etest->flags |= ETH_TEST_FL_FAILED;
return;
} else if (!netif_carrier_ok(dev)) {
netdev_err(priv->dev, "You need valid Link to execute tests\n");
etest->flags |= ETH_TEST_FL_FAILED;
return;
}
msleep(200);
for (i = 0; i < count; i++) {
ret = 0;
switch (stmmac_selftests[i].lb) {
case STMMAC_LOOPBACK_PHY:
ret = -EOPNOTSUPP;
if (dev->phydev)
ret = phy_loopback(dev->phydev, true, 0);
if (!ret)
break;
fallthrough;
case STMMAC_LOOPBACK_MAC:
ret = stmmac_set_mac_loopback(priv, priv->ioaddr, true);
break;
case STMMAC_LOOPBACK_NONE:
break;
default:
ret = -EOPNOTSUPP;
break;
}
if (ret) {
netdev_err(priv->dev, "Loopback is not supported\n");
etest->flags |= ETH_TEST_FL_FAILED;
break;
}
ret = stmmac_selftests[i].fn(priv);
if (ret && (ret != -EOPNOTSUPP))
etest->flags |= ETH_TEST_FL_FAILED;
buf[i] = ret;
switch (stmmac_selftests[i].lb) {
case STMMAC_LOOPBACK_PHY:
ret = -EOPNOTSUPP;
if (dev->phydev)
ret = phy_loopback(dev->phydev, false, 0);
if (!ret)
break;
fallthrough;
case STMMAC_LOOPBACK_MAC:
stmmac_set_mac_loopback(priv, priv->ioaddr, false);
break;
default:
break;
}
}
}
void stmmac_selftest_get_strings(struct stmmac_priv *priv, u8 *data)
{
u8 *p = data;
int i;
for (i = 0; i < stmmac_selftest_get_count(priv); i++) {
snprintf(p, ETH_GSTRING_LEN, "%2d. %s", i + 1,
stmmac_selftests[i].name);
p += ETH_GSTRING_LEN;
}
}
int stmmac_selftest_get_count(struct stmmac_priv *priv)
{
return ARRAY_SIZE(stmmac_selftests);
}
