net: stmmac: selftests: Add test for VLAN and Double VLAN Filtering

Add a selftest for VLAN and Double VLAN Filtering in stmmac.

Signed-off-by: Jose Abreu <joabreu@synopsys.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_selftests.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_selftests.c
index 83b775a8cedcb6ae02e7350491587b64ec8e3645..6b08bb15af159d7ca33ffbc9cab7917dee72c385 100644 (file)
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_selftests.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_selftests.c
@@ -715,6 +715,203 @@ static int stmmac_test_rss(struct stmmac_priv *priv)
        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;
+
+       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 (!ether_addr_equal(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;
+
+       if (!priv->dma_cap.vlhash)
+               return -EOPNOTSUPP;
+
+       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 = stmmac_test_vlan_validate;
+       tpriv->pt.dev = priv->dev;
+       tpriv->pt.af_packet_priv = tpriv;
+       tpriv->packet = &attr;
+
+       /*
+        * As we use HASH filtering, false positives may appear. This is a
+        * specially chosen ID so that adjacent IDs (+4) have different
+        * HASH values.
+        */
+       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;
+               }
+
+               skb_set_queue_mapping(skb, 0);
+               ret = dev_queue_xmit(skb);
+               if (ret)
+                       goto vlan_del;
+
+               wait_for_completion_timeout(&tpriv->comp, STMMAC_LB_TIMEOUT);
+               ret = !tpriv->ok;
+               if (ret && !i) {
+                       goto vlan_del;
+               } else if (!ret && i) {
+                       ret = -1;
+                       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_dvlanfilt(struct stmmac_priv *priv)
+{
+       struct stmmac_packet_attrs attr = { };
+       struct stmmac_test_priv *tpriv;
+       struct sk_buff *skb = NULL;
+       int ret = 0, i;
+
+       if (!priv->dma_cap.vlhash)
+               return -EOPNOTSUPP;
+
+       tpriv = kzalloc(sizeof(*tpriv), GFP_KERNEL);
+       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;
+
+       /*
+        * As we use HASH filtering, false positives may appear. This is a
+        * specially chosen ID so that adjacent IDs (+4) have different
+        * HASH values.
+        */
+       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;
+               }
+
+               skb_set_queue_mapping(skb, 0);
+               ret = dev_queue_xmit(skb);
+               if (ret)
+                       goto vlan_del;
+
+               wait_for_completion_timeout(&tpriv->comp, STMMAC_LB_TIMEOUT);
+               ret = !tpriv->ok;
+               if (ret && !i) {
+                       goto vlan_del;
+               } else if (!ret && i) {
+                       ret = -1;
+                       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;
+}
+
 #define STMMAC_LOOPBACK_NONE   0
 #define STMMAC_LOOPBACK_MAC    1
 #define STMMAC_LOOPBACK_PHY    2
@@ -764,6 +961,14 @@ static const struct stmmac_test {
                .name = "RSS                  ",
                .lb = STMMAC_LOOPBACK_PHY,
                .fn = stmmac_test_rss,
+       }, {
+               .name = "VLAN Filtering       ",
+               .lb = STMMAC_LOOPBACK_PHY,
+               .fn = stmmac_test_vlanfilt,
+       }, {
+               .name = "Double VLAN Filtering",
+               .lb = STMMAC_LOOPBACK_PHY,
+               .fn = stmmac_test_dvlanfilt,
        },
 };