#include <linux/bpf_trace.h>
 #include <linux/netdevice.h>
+#include <linux/bitfield.h>
 
 #include "../nfp_app.h"
 #include "../nfp_net.h"
 nfp_nfd3_parse_meta(struct net_device *netdev, struct nfp_meta_parsed *meta,
                    void *data, void *pkt, unsigned int pkt_len, int meta_len)
 {
-       u32 meta_info;
+       u32 meta_info, vlan_info;
 
        meta_info = get_unaligned_be32(data);
        data += 4;
                        meta->mark = get_unaligned_be32(data);
                        data += 4;
                        break;
+               case NFP_NET_META_VLAN:
+                       vlan_info = get_unaligned_be32(data);
+                       if (FIELD_GET(NFP_NET_META_VLAN_STRIP, vlan_info)) {
+                               meta->vlan.stripped = true;
+                               meta->vlan.tpid = FIELD_GET(NFP_NET_META_VLAN_TPID_MASK,
+                                                           vlan_info);
+                               meta->vlan.tci = FIELD_GET(NFP_NET_META_VLAN_TCI_MASK,
+                                                          vlan_info);
+                       }
+                       data += 4;
+                       break;
                case NFP_NET_META_PORTID:
                        meta->portid = get_unaligned_be32(data);
                        data += 4;
                }
 #endif
 
-               if (rxd->rxd.flags & PCIE_DESC_RX_VLAN)
-                       __vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q),
-                                              le16_to_cpu(rxd->rxd.vlan));
+               if (unlikely(!nfp_net_vlan_strip(skb, rxd, &meta))) {
+                       nfp_nfd3_rx_drop(dp, r_vec, rx_ring, NULL, skb);
+                       continue;
+               }
+
                if (meta_len_xdp)
                        skb_metadata_set(skb, meta_len_xdp);
 
 
         NFP_NET_CFG_CTRL_L2BC | NFP_NET_CFG_CTRL_L2MC |                \
         NFP_NET_CFG_CTRL_RXCSUM | NFP_NET_CFG_CTRL_TXCSUM |            \
         NFP_NET_CFG_CTRL_RXVLAN | NFP_NET_CFG_CTRL_TXVLAN |            \
+        NFP_NET_CFG_CTRL_RXVLAN_V2 | NFP_NET_CFG_CTRL_RXQINQ |         \
         NFP_NET_CFG_CTRL_GATHER | NFP_NET_CFG_CTRL_LSO |               \
         NFP_NET_CFG_CTRL_CTAG_FILTER | NFP_NET_CFG_CTRL_CMSG_DATA |    \
         NFP_NET_CFG_CTRL_RINGCFG | NFP_NET_CFG_CTRL_RSS |              \
 
 
        nfp_nfd3_rx_csum(dp, r_vec, rxd, meta, skb);
 
-       if (rxd->rxd.flags & PCIE_DESC_RX_VLAN)
-               __vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q),
-                                      le16_to_cpu(rxd->rxd.vlan));
+       if (unlikely(!nfp_net_vlan_strip(skb, rxd, meta))) {
+               dev_kfree_skb_any(skb);
+               nfp_net_xsk_rx_drop(r_vec, xrxbuf);
+               return;
+       }
+
        if (meta_xdp)
                skb_metadata_set(skb,
                                 xrxbuf->xdp->data - xrxbuf->xdp->data_meta);
 
 nfp_nfdk_parse_meta(struct net_device *netdev, struct nfp_meta_parsed *meta,
                    void *data, void *pkt, unsigned int pkt_len, int meta_len)
 {
-       u32 meta_info;
+       u32 meta_info, vlan_info;
 
        meta_info = get_unaligned_be32(data);
        data += 4;
                        meta->mark = get_unaligned_be32(data);
                        data += 4;
                        break;
+               case NFP_NET_META_VLAN:
+                       vlan_info = get_unaligned_be32(data);
+                       if (FIELD_GET(NFP_NET_META_VLAN_STRIP, vlan_info)) {
+                               meta->vlan.stripped = true;
+                               meta->vlan.tpid = FIELD_GET(NFP_NET_META_VLAN_TPID_MASK,
+                                                           vlan_info);
+                               meta->vlan.tci = FIELD_GET(NFP_NET_META_VLAN_TCI_MASK,
+                                                          vlan_info);
+                       }
+                       data += 4;
+                       break;
                case NFP_NET_META_PORTID:
                        meta->portid = get_unaligned_be32(data);
                        data += 4;
 
                nfp_nfdk_rx_csum(dp, r_vec, rxd, &meta, skb);
 
-               if (rxd->rxd.flags & PCIE_DESC_RX_VLAN)
-                       __vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q),
-                                              le16_to_cpu(rxd->rxd.vlan));
+               if (unlikely(!nfp_net_vlan_strip(skb, rxd, &meta))) {
+                       nfp_nfdk_rx_drop(dp, r_vec, rx_ring, NULL, skb);
+                       continue;
+               }
+
                if (meta_len_xdp)
                        skb_metadata_set(skb, meta_len_xdp);
 
 
         NFP_NET_CFG_CTRL_L2BC | NFP_NET_CFG_CTRL_L2MC |                \
         NFP_NET_CFG_CTRL_RXCSUM | NFP_NET_CFG_CTRL_TXCSUM |            \
         NFP_NET_CFG_CTRL_RXVLAN |                                      \
+        NFP_NET_CFG_CTRL_RXVLAN_V2 | NFP_NET_CFG_CTRL_RXQINQ |         \
         NFP_NET_CFG_CTRL_GATHER | NFP_NET_CFG_CTRL_LSO |               \
         NFP_NET_CFG_CTRL_CTAG_FILTER | NFP_NET_CFG_CTRL_CMSG_DATA |    \
         NFP_NET_CFG_CTRL_RINGCFG | NFP_NET_CFG_CTRL_IRQMOD |           \
 
 };
 
 #define NFP_NET_META_FIELD_MASK GENMASK(NFP_NET_META_FIELD_SIZE - 1, 0)
+#define NFP_NET_VLAN_CTAG      0
+#define NFP_NET_VLAN_STAG      1
 
 struct nfp_meta_parsed {
        u8 hash_type;
        u32 mark;
        u32 portid;
        __wsum csum;
+       struct {
+               bool stripped;
+               u8 tpid;
+               u16 tci;
+       } vlan;
 };
 
 struct nfp_net_rx_hash {
 
 
        if (changed & NETIF_F_HW_VLAN_CTAG_RX) {
                if (features & NETIF_F_HW_VLAN_CTAG_RX)
-                       new_ctrl |= NFP_NET_CFG_CTRL_RXVLAN;
+                       new_ctrl |= nn->cap & NFP_NET_CFG_CTRL_RXVLAN_V2 ?:
+                                   NFP_NET_CFG_CTRL_RXVLAN;
                else
-                       new_ctrl &= ~NFP_NET_CFG_CTRL_RXVLAN;
+                       new_ctrl &= ~NFP_NET_CFG_CTRL_RXVLAN_ANY;
        }
 
        if (changed & NETIF_F_HW_VLAN_CTAG_TX) {
                        new_ctrl &= ~NFP_NET_CFG_CTRL_CTAG_FILTER;
        }
 
+       if (changed & NETIF_F_HW_VLAN_STAG_RX) {
+               if (features & NETIF_F_HW_VLAN_STAG_RX)
+                       new_ctrl |= NFP_NET_CFG_CTRL_RXQINQ;
+               else
+                       new_ctrl &= ~NFP_NET_CFG_CTRL_RXQINQ;
+       }
+
        if (changed & NETIF_F_SG) {
                if (features & NETIF_F_SG)
                        new_ctrl |= NFP_NET_CFG_CTRL_GATHER;
        return 0;
 }
 
+static netdev_features_t
+nfp_net_fix_features(struct net_device *netdev,
+                    netdev_features_t features)
+{
+       if ((features & NETIF_F_HW_VLAN_CTAG_RX) &&
+           (features & NETIF_F_HW_VLAN_STAG_RX)) {
+               if (netdev->features & NETIF_F_HW_VLAN_CTAG_RX) {
+                       features &= ~NETIF_F_HW_VLAN_CTAG_RX;
+                       netdev->wanted_features &= ~NETIF_F_HW_VLAN_CTAG_RX;
+                       netdev_warn(netdev,
+                                   "S-tag and C-tag stripping can't be enabled at the same time. Enabling S-tag stripping and disabling C-tag stripping\n");
+               } else if (netdev->features & NETIF_F_HW_VLAN_STAG_RX) {
+                       features &= ~NETIF_F_HW_VLAN_STAG_RX;
+                       netdev->wanted_features &= ~NETIF_F_HW_VLAN_STAG_RX;
+                       netdev_warn(netdev,
+                                   "S-tag and C-tag stripping can't be enabled at the same time. Enabling C-tag stripping and disabling S-tag stripping\n");
+               }
+       }
+       return features;
+}
+
 static netdev_features_t
 nfp_net_features_check(struct sk_buff *skb, struct net_device *dev,
                       netdev_features_t features)
        .ndo_change_mtu         = nfp_net_change_mtu,
        .ndo_set_mac_address    = nfp_net_set_mac_address,
        .ndo_set_features       = nfp_net_set_features,
+       .ndo_fix_features       = nfp_net_fix_features,
        .ndo_features_check     = nfp_net_features_check,
        .ndo_get_phys_port_name = nfp_net_get_phys_port_name,
        .ndo_bpf                = nfp_net_xdp,
        .ndo_change_mtu         = nfp_net_change_mtu,
        .ndo_set_mac_address    = nfp_net_set_mac_address,
        .ndo_set_features       = nfp_net_set_features,
+       .ndo_fix_features       = nfp_net_fix_features,
        .ndo_features_check     = nfp_net_features_check,
        .ndo_get_phys_port_name = nfp_net_get_phys_port_name,
        .ndo_bpf                = nfp_net_xdp,
                nn->fw_ver.extend, nn->fw_ver.class,
                nn->fw_ver.major, nn->fw_ver.minor,
                nn->max_mtu);
-       nn_info(nn, "CAP: %#x %s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
+       nn_info(nn, "CAP: %#x %s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
                nn->cap,
                nn->cap & NFP_NET_CFG_CTRL_PROMISC  ? "PROMISC "  : "",
                nn->cap & NFP_NET_CFG_CTRL_L2BC     ? "L2BCFILT " : "",
                nn->cap & NFP_NET_CFG_CTRL_TXCSUM   ? "TXCSUM "   : "",
                nn->cap & NFP_NET_CFG_CTRL_RXVLAN   ? "RXVLAN "   : "",
                nn->cap & NFP_NET_CFG_CTRL_TXVLAN   ? "TXVLAN "   : "",
+               nn->cap & NFP_NET_CFG_CTRL_RXQINQ   ? "RXQINQ "   : "",
+               nn->cap & NFP_NET_CFG_CTRL_RXVLAN_V2 ? "RXVLANv2 "   : "",
                nn->cap & NFP_NET_CFG_CTRL_SCATTER  ? "SCATTER "  : "",
                nn->cap & NFP_NET_CFG_CTRL_GATHER   ? "GATHER "   : "",
                nn->cap & NFP_NET_CFG_CTRL_LSO      ? "TSO1 "     : "",
 
        netdev->vlan_features = netdev->hw_features;
 
-       if (nn->cap & NFP_NET_CFG_CTRL_RXVLAN) {
+       if (nn->cap & NFP_NET_CFG_CTRL_RXVLAN_ANY) {
                netdev->hw_features |= NETIF_F_HW_VLAN_CTAG_RX;
-               nn->dp.ctrl |= NFP_NET_CFG_CTRL_RXVLAN;
+               nn->dp.ctrl |= nn->cap & NFP_NET_CFG_CTRL_RXVLAN_V2 ?:
+                              NFP_NET_CFG_CTRL_RXVLAN;
        }
        if (nn->cap & NFP_NET_CFG_CTRL_TXVLAN) {
                if (nn->cap & NFP_NET_CFG_CTRL_LSO2) {
                netdev->hw_features |= NETIF_F_HW_VLAN_CTAG_FILTER;
                nn->dp.ctrl |= NFP_NET_CFG_CTRL_CTAG_FILTER;
        }
+       if (nn->cap & NFP_NET_CFG_CTRL_RXQINQ) {
+               netdev->hw_features |= NETIF_F_HW_VLAN_STAG_RX;
+               nn->dp.ctrl |= NFP_NET_CFG_CTRL_RXQINQ;
+       }
 
        netdev->features = netdev->hw_features;
 
        if (nfp_app_has_tc(nn->app) && nn->port)
                netdev->hw_features |= NETIF_F_HW_TC;
 
-       /* Advertise but disable TSO by default. */
-       netdev->features &= ~(NETIF_F_TSO | NETIF_F_TSO6);
-       nn->dp.ctrl &= ~NFP_NET_CFG_CTRL_LSO_ANY;
+       /* Advertise but disable TSO by default.
+        * C-Tag strip and S-Tag strip can't be supported simultaneously,
+        * so enable C-Tag strip and disable S-Tag strip by default.
+        */
+       netdev->features &= ~(NETIF_F_TSO | NETIF_F_TSO6 | NETIF_F_HW_VLAN_STAG_RX);
+       nn->dp.ctrl &= ~(NFP_NET_CFG_CTRL_LSO_ANY | NFP_NET_CFG_CTRL_RXQINQ);
 
        /* Finalise the netdev setup */
        switch (nn->dp.ops->version) {
 
 #define NFP_NET_LSO_MAX_HDR_SZ         255
 #define NFP_NET_LSO_MAX_SEGS           64
 
+/* working with metadata vlan api (NFD version >= 2.0) */
+#define NFP_NET_META_VLAN_STRIP                        BIT(31)
+#define NFP_NET_META_VLAN_TPID_MASK            GENMASK(19, 16)
+#define NFP_NET_META_VLAN_TCI_MASK             GENMASK(15, 0)
+
 /* Prepend field types */
 #define NFP_NET_META_FIELD_SIZE                4
 #define NFP_NET_META_HASH              1 /* next field carries hash type */
 #define NFP_NET_META_MARK              2
+#define NFP_NET_META_VLAN              4 /* ctag or stag type */
 #define NFP_NET_META_PORTID            5
 #define NFP_NET_META_CSUM              6 /* checksum complete type */
 #define NFP_NET_META_CONN_HANDLE       7
 #define   NFP_NET_CFG_CTRL_LSO           (0x1 << 10) /* LSO/TSO (version 1) */
 #define   NFP_NET_CFG_CTRL_CTAG_FILTER   (0x1 << 11) /* VLAN CTAG filtering */
 #define   NFP_NET_CFG_CTRL_CMSG_DATA     (0x1 << 12) /* RX cmsgs on data Qs */
+#define   NFP_NET_CFG_CTRL_RXQINQ        (0x1 << 13) /* Enable S-tag strip */
+#define   NFP_NET_CFG_CTRL_RXVLAN_V2     (0x1 << 15) /* Enable C-tag strip */
 #define   NFP_NET_CFG_CTRL_RINGCFG       (0x1 << 16) /* Ring runtime changes */
 #define   NFP_NET_CFG_CTRL_RSS           (0x1 << 17) /* RSS (version 1) */
 #define   NFP_NET_CFG_CTRL_IRQMOD        (0x1 << 18) /* Interrupt moderation */
                                         NFP_NET_CFG_CTRL_CSUM_COMPLETE)
 #define NFP_NET_CFG_CTRL_CHAIN_META    (NFP_NET_CFG_CTRL_RSS2 | \
                                         NFP_NET_CFG_CTRL_CSUM_COMPLETE)
+#define NFP_NET_CFG_CTRL_RXVLAN_ANY    (NFP_NET_CFG_CTRL_RXVLAN | \
+                                        NFP_NET_CFG_CTRL_RXVLAN_V2)
 
 #define NFP_NET_CFG_UPDATE             0x0004
 #define   NFP_NET_CFG_UPDATE_GEN         (0x1 <<  0) /* General update */
 
 
        return ret;
 }
+
+bool nfp_net_vlan_strip(struct sk_buff *skb, const struct nfp_net_rx_desc *rxd,
+                       const struct nfp_meta_parsed *meta)
+{
+       u16 tpid = 0, tci = 0;
+
+       if (rxd->rxd.flags & PCIE_DESC_RX_VLAN) {
+               tpid = ETH_P_8021Q;
+               tci = le16_to_cpu(rxd->rxd.vlan);
+       } else if (meta->vlan.stripped) {
+               if (meta->vlan.tpid == NFP_NET_VLAN_CTAG)
+                       tpid = ETH_P_8021Q;
+               else if (meta->vlan.tpid == NFP_NET_VLAN_STAG)
+                       tpid = ETH_P_8021AD;
+               else
+                       return false;
+
+               tci = meta->vlan.tci;
+       }
+       if (tpid)
+               __vlan_hwaccel_put_tag(skb, htons(tpid), tci);
+
+       return true;
+}
 
 void nfp_net_rx_rings_free(struct nfp_net_dp *dp);
 void nfp_net_tx_rings_free(struct nfp_net_dp *dp);
 void nfp_net_rx_ring_reset(struct nfp_net_rx_ring *rx_ring);
+bool nfp_net_vlan_strip(struct sk_buff *skb, const struct nfp_net_rx_desc *rxd,
+                       const struct nfp_meta_parsed *meta);
 
 enum nfp_nfd_version {
        NFP_NFD_VER_NFD3,
 
 
        netdev->vlan_features = netdev->hw_features;
 
-       if (repr_cap & NFP_NET_CFG_CTRL_RXVLAN)
+       if (repr_cap & NFP_NET_CFG_CTRL_RXVLAN_ANY)
                netdev->hw_features |= NETIF_F_HW_VLAN_CTAG_RX;
        if (repr_cap & NFP_NET_CFG_CTRL_TXVLAN) {
                if (repr_cap & NFP_NET_CFG_CTRL_LSO2)
        }
        if (repr_cap & NFP_NET_CFG_CTRL_CTAG_FILTER)
                netdev->hw_features |= NETIF_F_HW_VLAN_CTAG_FILTER;
+       if (repr_cap & NFP_NET_CFG_CTRL_RXQINQ)
+               netdev->hw_features |= NETIF_F_HW_VLAN_STAG_RX;
 
        netdev->features = netdev->hw_features;
 
-       /* Advertise but disable TSO by default. */
-       netdev->features &= ~(NETIF_F_TSO | NETIF_F_TSO6);
+       /* Advertise but disable TSO by default.
+        * C-Tag strip and S-Tag strip can't be supported simultaneously,
+        * so enable C-Tag strip and disable S-Tag strip by default.
+        */
+       netdev->features &= ~(NETIF_F_TSO | NETIF_F_TSO6 | NETIF_F_HW_VLAN_STAG_RX);
        netif_set_tso_max_segs(netdev, NFP_NET_LSO_MAX_SEGS);
 
        netdev->priv_flags |= IFF_NO_QUEUE | IFF_DISABLE_NETPOLL;